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 "canethgw.h"
  13
  14 MODULE_LICENSE( "GPL" );
  15
  16 static struct task_struct* eth_to_can, * can_to_eth;
  17 static struct socket* udp_sock;
  18 static struct socket* can_sock;
  19 static struct net_device* can_dev;
  20
  21 /***********************
  22  *   UDP
  23  ***********************/
  24
  25 int gw_udp_recv( void* data )
  26 {
  27         struct can_frame cf;
  28         struct kvec vec;
  29         struct msghdr mh;
  30
  31         vec.iov_base = &cf;
  32         vec.iov_len = sizeof(cf);
  33
  34         mh.msg_name = NULL;
  35         mh.msg_namelen = 0;
  36         mh.msg_iov = NULL;
  37         mh.msg_iovlen = 0;
  38         mh.msg_control = NULL;
  39         mh.msg_controllen = 0;
  40         mh.msg_flags = 0;
  41
  42         while( 1 )
  43         {
  44                 if( kthread_should_stop() ) /* up() ?, recv is blocking */
  45                         break;
  46                 kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* todo: handle error */
  47                 printk( "received udp msg_id:%d\n", cf.can_id );
  48                 gw_can_send( &cf );
  49         }
  50
  51         return 0;
  52 }
  53
  54 void gw_udp_send( struct can_frame* cf )
  55 {
  56         struct msghdr mh;
  57         struct sockaddr_in addr;
  58         struct kvec vec;
  59
  60         addr.sin_family = AF_INET;
  61         addr.sin_port = htons( 10502 );
  62         addr.sin_addr.s_addr = 0x0100007f;
  63
  64         mh.msg_name = &addr;
  65         mh.msg_namelen = sizeof( addr );
  66         mh.msg_control = NULL;
  67         mh.msg_controllen = 0;
  68         mh.msg_flags = 0;
  69
  70         vec.iov_base = cf;
  71         vec.iov_len = sizeof( *cf );
  72
  73         kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
  74 }
  75
  76 /***********************
  77  *   CAN
  78  ***********************/
  79
  80 int gw_can_recv( void* data )
  81 {
  82         struct msghdr mh;
  83         struct kvec vec;
  84         struct can_frame cf;
  85
  86         mh.msg_name = NULL;
  87         mh.msg_namelen = 0;
  88         mh.msg_control = NULL;
  89         mh.msg_controllen = 0;
  90         mh.msg_flags = 0;
  91
  92         vec.iov_base = &cf;
  93         vec.iov_len = sizeof( cf );
  94
  95         while( 1 )
  96         {
  97                 if( kthread_should_stop() ) /**/
  98                         break;
  99                 kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
 100                 printk( "received can msg_id:%d\n", cf.can_id );
 101                 gw_udp_send( &cf );
 102         }
 103
 104         return 0;
 105 }
 106
 107 void gw_can_send( struct can_frame* cf )
 108 {
 109         struct msghdr mh;
 110         struct kvec vec;
 111
 112         mh.msg_name = NULL;
 113         mh.msg_namelen = 0;
 114         mh.msg_control = NULL;
 115         mh.msg_controllen = 0;
 116         mh.msg_flags = 0;
 117
 118         vec.iov_base = cf;
 119         vec.iov_len = sizeof( *cf );
 120
 121         kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) );
 122 }
 123
 124 /***********************
 125  *   module init/exit
 126  ***********************/
 127
 128 static int __init cangw_init( void )
 129 {
 130         struct sockaddr_in udp_addr;
 131         struct sockaddr_can can_addr;
 132         int ifidx = 0;
 133
 134         /* 1. create can socket and bind to it */
 135         can_dev = dev_get_by_name( &init_net, "vcan0" ); /* net ns?, release counter! */
 136         if( can_dev == NULL )
 137         {
 138                 printk( KERN_ERR "error: vcan0 not found\n" );
 139                 return -1;
 140         }
 141         ifidx = can_dev->ifindex;
 142         dev_put( can_dev );
 143
 144         if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
 145         {
 146                 printk( KERN_ERR "error: can_sock creation failed\n" );
 147                 return -1;
 148         }
 149
 150         can_addr.can_family = AF_CAN;
 151         can_addr.can_ifindex = ifidx;
 152
 153         if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
 154         {
 155                 printk( KERN_ERR "can_sock bind failed\n" );
 156                 return -1;
 157         }
 158
 159         /* 2. create udp socket and bind to it */
 160         if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
 161         {
 162                 printk( "error: udp_sock creation failed\n" );
 163                 sock_release( can_sock );
 164                 return -1;
 165         }
 166
 167         udp_addr.sin_family = AF_INET;
 168         udp_addr.sin_port = htons( 10501 );
 169         udp_addr.sin_addr.s_addr = INADDR_ANY;
 170
 171         if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
 172         {
 173                 printk( "error: binding failed\n" );
 174                 sock_release( udp_sock );
 175                 sock_release( can_sock );
 176                 return -1;
 177         }
 178
 179         /* 3. run bridging threads */
 180         eth_to_can = kthread_run( gw_udp_recv, NULL, "cangw" );
 181         can_to_eth = kthread_run( gw_can_recv, NULL, "cangw" );
 182
 183         /*
 184         if( sock_create_kern( AF_CAN, SOCK_RAW, CAN_RAW, &can_sock ) != 0 )
 185         {s
 186                 printk( "error: can_sock creation failed\n" );
 187         }
 188         */
 189
 190         return 0;
 191 }
 192
 193 static void __exit cangw_exit( void )
 194 {
 195         sock_release( udp_sock );
 196         sock_release( can_sock );
 197
 198         printk( "cangw: exit\n" );
 199         //kthread_stop( ts );
 200 }
 201
 202 module_init( cangw_init );
 203 module_exit( cangw_exit );