From 08d097af8e481957b9f2e4751e9d29553c18e995 Mon Sep 17 00:00:00 2001 From: Radek Matejka Date: Thu, 19 Jul 2012 11:45:14 +0200 Subject: [PATCH] canethgw (kernel space) first commit Routes all from can to udp and vice versa. Listens on udp port 10501, sends to 10502. --- kernel/Makefile | 12 +++ kernel/canethgw.c | 203 ++++++++++++++++++++++++++++++++++++++++++++++ kernel/canethgw.h | 5 ++ 3 files changed, 220 insertions(+) create mode 100644 kernel/Makefile create mode 100644 kernel/canethgw.c create mode 100644 kernel/canethgw.h diff --git a/kernel/Makefile b/kernel/Makefile new file mode 100644 index 0000000..b7a37f6 --- /dev/null +++ b/kernel/Makefile @@ -0,0 +1,12 @@ +obj-m += canethgw.o + +all: + make -C /lib/modules/`uname -r`/build M=`pwd` +install: + make -C /lib/modules/`uname -r`/build M=`pwd` modules_install +clean: + make -C /lib/modules/`uname -r`/build M=`pwd` clean +test: + modprobe -r canethgw + modprobe canethgw + diff --git a/kernel/canethgw.c b/kernel/canethgw.c new file mode 100644 index 0000000..a3307bf --- /dev/null +++ b/kernel/canethgw.c @@ -0,0 +1,203 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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 ); diff --git a/kernel/canethgw.h b/kernel/canethgw.h new file mode 100644 index 0000000..2005bab --- /dev/null +++ b/kernel/canethgw.h @@ -0,0 +1,5 @@ +int gw_udp_recv( void* data ); +void gw_udp_send( struct can_frame* cf ); +int gw_can_recv( void* data ); +void gw_can_send( struct can_frame* cf ); + -- 2.39.2