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 /***********************
  25  *   UDP
  26  ***********************/
  27
  28 static int gw_udp_recv( void* data )
  29 {
  30         struct can_frame cf;
  31         struct kvec vec;
  32         struct msghdr mh;
  33
  34         vec.iov_base = &cf;
  35         vec.iov_len = sizeof(cf);
  36
  37         mh.msg_name = NULL;
  38         mh.msg_namelen = 0;
  39         mh.msg_iov = NULL;
  40         mh.msg_iovlen = 0;
  41         mh.msg_control = NULL;
  42         mh.msg_controllen = 0;
  43         mh.msg_flags = 0;
  44
  45         while( 1 )
  46         {
  47                 if( kthread_should_stop() ) /* up() ?, recv is blocking */
  48                         break;
  49                 kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* todo: handle error */
  50                 printk( "received udp msg_id:%d\n", cf.can_id );
  51                 gw_can_send( &cf );
  52         }
  53
  54         return 0;
  55 }
  56
  57 static void gw_udp_send( struct can_frame* cf )
  58 {
  59         struct msghdr mh;
  60         struct sockaddr_in addr;
  61         struct kvec vec;
  62
  63         addr.sin_family = AF_INET;
  64         addr.sin_port = htons( 10502 );
  65         addr.sin_addr.s_addr = 0x0100007f;
  66
  67         mh.msg_name = &addr;
  68         mh.msg_namelen = sizeof( addr );
  69         mh.msg_control = NULL;
  70         mh.msg_controllen = 0;
  71         mh.msg_flags = 0;
  72
  73         vec.iov_base = cf;
  74         vec.iov_len = sizeof( *cf );
  75
  76         kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
  77 }
  78
  79 /***********************
  80  *   CAN
  81  ***********************/
  82
  83 static int gw_can_recv( void* data )
  84 {
  85         struct msghdr mh;
  86         struct kvec vec;
  87         struct can_frame cf;
  88
  89         mh.msg_name = NULL;
  90         mh.msg_namelen = 0;
  91         mh.msg_control = NULL;
  92         mh.msg_controllen = 0;
  93         mh.msg_flags = 0;
  94
  95         vec.iov_base = &cf;
  96         vec.iov_len = sizeof( cf );
  97
  98         while( 1 )
  99         {
 100                 if( kthread_should_stop() ) /**/
 101                         break;
 102                 kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
 103                 printk( "received can msg_id:%d\n", cf.can_id );
 104                 gw_udp_send( &cf );
 105         }
 106
 107         return 0;
 108 }
 109
 110 static void gw_can_send( struct can_frame* cf )
 111 {
 112         struct msghdr mh;
 113         struct kvec vec;
 114
 115         mh.msg_name = NULL;
 116         mh.msg_namelen = 0;
 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( can_sock, &mh, &vec, 1, sizeof( *cf ) );
 125 }
 126
 127 /**/
 128
 129 static int cgw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh,
 130                      void* arg )
 131 {
 132         struct nlattr* tb[ CGW_MAX+1 ];
 133         struct rtcanmsg *r;
 134         struct cgw_job *gwj;
 135         int err = 0;
 136
 137         if (nlmsg_len(nlh) < sizeof(*r))
 138                 return -EINVAL;
 139
 140         r = nlmsg_data(nlh);
 141         if (r->can_family != AF_CAN)
 142                 return -EPFNOSUPPORT;
 143
 144         /* so far we only support CAN -> CAN routings */
 145         if (r->gwtype == CGW_TYPE_CAN_ETH)
 146                 printk( KERN_INFO "canethgw: TYPE_CAN_ETH" );
 147         else
 148                 return -1;
 149
 150         err = nlmsg_parse( nlh, sizeof( struct rtcanmsg ), tb, CGW_MAX, NULL );
 151         if( err < 0 )
 152                 return err;
 153
 154         printk( KERN_INFO "src port:%u\n", *(u16*)nla_data( tb[CGW_SRC_PORT] ) );
 155         printk( KERN_INFO "dest port:%u\n", *(u16*)nla_data( tb[CGW_DST_PORT] ) );
 156
 157         return 0;
 158 }
 159
 160 /***********************
 161  *   module init/exit
 162  ***********************/
 163
 164 static int __init cangw_init( void )
 165 {
 166         struct sockaddr_in udp_addr;
 167         struct sockaddr_can can_addr;
 168         int ifidx = 0;
 169
 170         /* 1. create can socket and bind to it */
 171         can_dev = dev_get_by_name( &init_net, "vcan0" ); /* net ns?, release counter! */
 172         if( can_dev == NULL )
 173         {
 174                 printk( KERN_ERR "error: vcan0 not found\n" );
 175                 return -1;
 176         }
 177         ifidx = can_dev->ifindex;
 178         dev_put( can_dev );
 179
 180         if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
 181         {
 182                 printk( KERN_ERR "error: can_sock creation failed\n" );
 183                 return -1;
 184         }
 185
 186         can_addr.can_family = AF_CAN;
 187         can_addr.can_ifindex = ifidx;
 188
 189         if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
 190         {
 191                 printk( KERN_ERR "can_sock bind failed\n" );
 192                 return -1;
 193         }
 194
 195         /* 2. create udp socket and bind to it */
 196         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
 197         {
 198                 printk( "error: udp_sock creation failed\n" );
 199                 sock_release( can_sock );
 200                 return -1;
 201         }
 202
 203         udp_addr.sin_family = AF_INET;
 204         udp_addr.sin_port = htons( 10501 );
 205         udp_addr.sin_addr.s_addr = INADDR_ANY;
 206
 207         if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
 208         {
 209                 printk( "error: binding failed\n" );
 210                 sock_release( udp_sock );
 211                 sock_release( can_sock );
 212                 return -1;
 213         }
 214
 215         /* 3. subscribe to netlink */
 216         //if( __rtnl_register( PF_CAN, RTM_GETROUTE,  ) != 0 )
 217         __rtnl_register( PF_CAN, RTM_NEWROUTE, cgw_create_job, NULL, NULL );
 218         //__rtnl_register( PF_CAN, RTM_DELROUTE,  )
 219
 220         /* 4. run threads */
 221         eth_to_can = kthread_run( gw_udp_recv, NULL, "cangw" );
 222         can_to_eth = kthread_run( gw_can_recv, NULL, "cangw" );
 223
 224         /*
 225         if( sock_create_kern( AF_CAN, SOCK_RAW, CAN_RAW, &can_sock ) != 0 )
 226         {s
 227                 printk( "error: can_sock creation failed\n" );
 228         }
 229         */
 230
 231         return 0;
 232 }
 233
 234 static void __exit cangw_exit( void )
 235 {
 236         sock_release( udp_sock );
 237         sock_release( can_sock );
 238
 239         printk( "cangw: exit\n" );
 240         //kthread_stop( ts );
 241 }
 242
 243 module_init( cangw_init );
 244 module_exit( cangw_exit );