+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+#include <linux/wait.h>
+#include <linux/netdevice.h>
+#include <linux/socket.h>
+#include <linux/net.h>
+#include <linux/can/core.h>
+#include <linux/can.h>
+#include "canethgw.h"
+
+MODULE_LICENSE( "GPL" );
+
+static struct task_struct* eth_to_can, * can_to_eth;
+static struct socket* udp_sock;
+static struct socket* can_sock;
+static struct net_device* can_dev;
+
+/***********************
+ * UDP
+ ***********************/
+
+int gw_udp_recv( void* data )
+{
+ struct can_frame cf;
+ struct kvec vec;
+ struct msghdr mh;
+
+ vec.iov_base = &cf;
+ vec.iov_len = sizeof(cf);
+
+ mh.msg_name = NULL;
+ mh.msg_namelen = 0;
+ mh.msg_iov = NULL;
+ mh.msg_iovlen = 0;
+ mh.msg_control = NULL;
+ mh.msg_controllen = 0;
+ mh.msg_flags = 0;
+
+ while( 1 )
+ {
+ if( kthread_should_stop() ) /* up() ?, recv is blocking */
+ break;
+ kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* todo: handle error */
+ printk( "received udp msg_id:%d\n", cf.can_id );
+ gw_can_send( &cf );
+ }
+
+ return 0;
+}
+
+void gw_udp_send( struct can_frame* cf )
+{
+ struct msghdr mh;
+ struct sockaddr_in addr;
+ struct kvec vec;
+
+ addr.sin_family = AF_INET;
+ addr.sin_port = htons( 10502 );
+ addr.sin_addr.s_addr = 0x0100007f;
+
+ mh.msg_name = &addr;
+ mh.msg_namelen = sizeof( addr );
+ mh.msg_control = NULL;
+ mh.msg_controllen = 0;
+ mh.msg_flags = 0;
+
+ vec.iov_base = cf;
+ vec.iov_len = sizeof( *cf );
+
+ kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
+}
+
+/***********************
+ * CAN
+ ***********************/
+
+int gw_can_recv( void* data )
+{
+ struct msghdr mh;
+ struct kvec vec;
+ struct can_frame cf;
+
+ mh.msg_name = NULL;
+ mh.msg_namelen = 0;
+ mh.msg_control = NULL;
+ mh.msg_controllen = 0;
+ mh.msg_flags = 0;
+
+ vec.iov_base = &cf;
+ vec.iov_len = sizeof( cf );
+
+ while( 1 )
+ {
+ if( kthread_should_stop() ) /**/
+ break;
+ kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
+ printk( "received can msg_id:%d\n", cf.can_id );
+ gw_udp_send( &cf );
+ }
+
+ return 0;
+}
+
+void gw_can_send( struct can_frame* cf )
+{
+ struct msghdr mh;
+ struct kvec vec;
+
+ mh.msg_name = NULL;
+ mh.msg_namelen = 0;
+ mh.msg_control = NULL;
+ mh.msg_controllen = 0;
+ mh.msg_flags = 0;
+
+ vec.iov_base = cf;
+ vec.iov_len = sizeof( *cf );
+
+ kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) );
+}
+
+/***********************
+ * module init/exit
+ ***********************/
+
+static int __init cangw_init( void )
+{
+ struct sockaddr_in udp_addr;
+ struct sockaddr_can can_addr;
+ int ifidx = 0;
+
+ /* 1. create can socket and bind to it */
+ can_dev = dev_get_by_name( &init_net, "vcan0" ); /* net ns?, release counter! */
+ if( can_dev == NULL )
+ {
+ printk( KERN_ERR "error: vcan0 not found\n" );
+ return -1;
+ }
+ ifidx = can_dev->ifindex;
+ dev_put( can_dev );
+
+ if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
+ {
+ printk( KERN_ERR "error: can_sock creation failed\n" );
+ return -1;
+ }
+
+ can_addr.can_family = AF_CAN;
+ can_addr.can_ifindex = ifidx;
+
+ if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
+ {
+ printk( KERN_ERR "can_sock bind failed\n" );
+ return -1;
+ }
+
+ /* 2. create udp socket and bind to it */
+ if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
+ {
+ printk( "error: udp_sock creation failed\n" );
+ sock_release( can_sock );
+ return -1;
+ }
+
+ udp_addr.sin_family = AF_INET;
+ udp_addr.sin_port = htons( 10501 );
+ udp_addr.sin_addr.s_addr = INADDR_ANY;
+
+ if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
+ {
+ printk( "error: binding failed\n" );
+ sock_release( udp_sock );
+ sock_release( can_sock );
+ return -1;
+ }
+
+ /* 3. run bridging threads */
+ eth_to_can = kthread_run( gw_udp_recv, NULL, "cangw" );
+ can_to_eth = kthread_run( gw_can_recv, NULL, "cangw" );
+
+ /*
+ if( sock_create_kern( AF_CAN, SOCK_RAW, CAN_RAW, &can_sock ) != 0 )
+ {s
+ printk( "error: can_sock creation failed\n" );
+ }
+ */
+
+ return 0;
+}
+
+static void __exit cangw_exit( void )
+{
+ sock_release( udp_sock );
+ sock_release( can_sock );
+
+ printk( "cangw: exit\n" );
+ //kthread_stop( ts );
+}
+
+module_init( cangw_init );
+module_exit( cangw_exit );