+#define DEBUG 1
+
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/kthread.h>
#include <linux/net.h>
#include <linux/can/core.h>
#include <linux/can.h>
+#include <net/rtnetlink.h>
+#include <net/sock.h>
#include "canethgw.h"
+#include <linux/completion.h>
+#include <linux/mutex.h>
+#include <net/inet_common.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
- ***********************/
+static int cegw_udp_can( void* data );
+inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port );
+static int cegw_can_udp( void* data );
+inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex );
+static int cegw_thread_start( void* data );
+static int cegw_thread_stop( void );
-int gw_udp_recv( void* data )
+enum __cegw_state
{
- struct can_frame cf;
- struct kvec vec;
- struct msghdr mh;
+ CEGW_RUN,
+ CEGW_STOP,
+ CEGW_EXIT
+};
- vec.iov_base = &cf;
- vec.iov_len = sizeof(cf);
+struct cegw_rule
+{
+ int can_ifindex;
+ struct in_addr eth_ip;
+ unsigned short eth_port;
+ struct hlist_node list;
+};
- 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;
+struct cegw_setting
+{
+ struct in_addr eth_ip;
+ unsigned short eth_port;
+};
- 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 );
- }
+static int cegw_state = CEGW_STOP;
+static struct socket* can_sock = NULL, * udp_sock = NULL;
+static struct task_struct* eth_to_can = NULL, * can_to_eth = NULL;
- return 0;
-}
+HLIST_HEAD( cegw_rule_can_eth );
+HLIST_HEAD( cegw_rule_eth_can );
+DEFINE_MUTEX( cegw_mutex );
-void gw_udp_send( struct can_frame* cf )
+inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port )
{
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;
+ addr.sin_port = htons( port );
+ addr.sin_addr = ipaddr;
mh.msg_name = &addr;
mh.msg_namelen = sizeof( addr );
kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
}
-/***********************
- * CAN
- ***********************/
-
-int gw_can_recv( void* data )
+inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex )
{
struct msghdr mh;
struct kvec vec;
- struct can_frame cf;
+ struct sockaddr_can addr;
- mh.msg_name = NULL;
- mh.msg_namelen = 0;
+ addr.can_family = AF_CAN;
+ addr.can_ifindex = ifindex;
+
+ 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 );
+ vec.iov_base = cf;
+ vec.iov_len = sizeof( *cf );
+ kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) ); /* ToDo: handle error */
+}
+
+/**
+ * cegw_udp_can - performs udp->can routing
+ * This function is run as a thread.
+ */
+static int cegw_udp_can( void* data )
+{
+ struct can_frame cf;
+ struct kvec vec;
+ struct msghdr mh;
+ struct cegw_rule* job;
+ struct hlist_node* pos;
+ int can_ifidx;
+ int recv_size;
+
+ 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() ) /**/
+ if( cegw_state == CEGW_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 );
+ vec.iov_base = &cf;
+ vec.iov_len = sizeof(cf);
+ recv_size = kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* ToDo: handle error, size check */
+ if( recv_size == 0 )
+ {
+ continue;
+ }
+ hlist_for_each_entry_rcu( job, pos, &cegw_rule_eth_can, list )
+ {
+ rcu_read_lock(); /**/
+ can_ifidx = job->can_ifindex;
+ rcu_read_unlock();
+ /* ToDo: from filter */
+ cegw_can_send( can_sock, &cf, can_ifidx );
+ }
}
-
+
return 0;
}
-void gw_can_send( struct can_frame* cf )
+/**
+ * cegw_can_udp - performs can->udp routing
+ */
+static int cegw_can_udp( void* data )
{
struct msghdr mh;
- struct kvec vec;
-
- mh.msg_name = NULL;
- mh.msg_namelen = 0;
+ struct kvec vec;
+ struct can_frame cf;
+ struct sockaddr_can ca;
+ struct cegw_rule* rule;
+ struct hlist_node* pos;
+ struct in_addr eth_ip;
+ u16 eth_port;
+ int recv_size;
+
+ mh.msg_name = &ca;
+ mh.msg_namelen = sizeof( ca );
mh.msg_control = NULL;
mh.msg_controllen = 0;
mh.msg_flags = 0;
+
+ while( 1 )
+ {
+ if( cegw_state == CEGW_STOP ) /**/
+ break;
+ vec.iov_base = &cf;
+ vec.iov_len = sizeof( cf );
+
+ recv_size = kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
+ if( recv_size == 0 )
+ {
+ continue;
+ }
+ hlist_for_each_entry_rcu( rule, pos, &cegw_rule_can_eth, list )
+ {
+ rcu_read_lock();
+ eth_ip = rule->eth_ip;
+ eth_port = rule->eth_port;
+ rcu_read_unlock();
+ if( rule->can_ifindex == ca.can_ifindex )
+ cegw_udp_send( udp_sock, &cf, eth_ip, eth_port );
+ }
+ }
- vec.iov_base = cf;
- vec.iov_len = sizeof( *cf );
+ return 0;
+}
+
+/* NetLink */
+
+static int cegw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
+{
+ struct nlattr* tb[ CEGW_MAX+1 ];
+ struct cegw_rule* rule = NULL;
+ int ifindex;
+ struct rtmsg* r;
+ struct in_addr ip;
+ unsigned short port;
+ struct cegw_setting* set;
+ int err = 0;
- kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) );
+ if( nlmsg_len(nlh) < sizeof(*r) )
+ return -EINVAL;
+
+ r = nlmsg_data( nlh );
+
+ if( r->rtm_family != AF_CAN )
+ return -EPFNOSUPPORT;
+
+ err = nlmsg_parse( nlh, sizeof(*r), tb, CEGW_MAX, NULL );
+ if( err < 0 )
+ {
+ pr_devel( "canethgw: nlmsg_parse error\n" );
+ return err;
+ }
+
+ if( tb[CEGW_CMD_INFO] == NULL )
+ {
+ pr_devel( "canethgw: CEGW_CMD_INFO is missing in rtmsg\n" );
+ return -EINVAL;
+ }
+
+ switch( *(int*)nla_data( tb[CEGW_CMD_INFO] ) )
+ {
+ case CEGW_LISTEN:
+ if( !tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] )
+ {
+ pr_devel( "canethgw: missing attribute for CEGW_LISTEN\n" );
+ return -EINVAL;
+ }
+
+ /* ToDo: valid listen address */
+ set = kmalloc( sizeof(*set), GFP_KERNEL );
+ set->eth_ip = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
+ set->eth_port = *(unsigned short*)nla_data( tb[CEGW_ETH_PORT] );
+ kthread_run( cegw_thread_start, set, "canethgw" );
+ break;
+ case CEGW_RULE_CAN_ETH:
+ if( !tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] || !tb[CEGW_CAN_IFINDEX] )
+ {
+ pr_devel( "canethgw: missing attribute for CEGW_RULE_CAN_ETH\n" );
+ return -EINVAL;
+ }
+
+ ifindex = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] );
+ ip = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
+ port = *(unsigned short*)nla_data( tb[CEGW_ETH_PORT] );
+ pr_devel( "canethgw: new can->eth rule - (%d)->(%x:%hu)\n", ifindex, ip.s_addr, port );
+
+ rule = kmalloc( sizeof(struct cegw_rule), GFP_KERNEL );
+ if( rule == NULL )
+ {
+ break;
+ }
+
+ rule->can_ifindex = ifindex;
+ rule->eth_ip = ip;
+ rule->eth_port = port;
+
+ hlist_add_head_rcu( &rule->list, &cegw_rule_can_eth );
+ break;
+ case CEGW_RULE_ETH_CAN:
+ if( !tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] || !tb[CEGW_CAN_IFINDEX] )
+ {
+ pr_devel( "canethgw: missing attribute for CEGW_RULE_ETH_CAN\n" );
+ return -EINVAL;
+ }
+
+ ifindex = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] );
+ ip = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
+ port = *(unsigned short*)nla_data( tb[CEGW_ETH_PORT] );
+ pr_devel( "canethgw: new eth->can rule - (%x:%hu)->(%d)\n", ip.s_addr, port, ifindex );
+
+ rule = kmalloc( sizeof(struct cegw_rule), GFP_KERNEL );
+ if( rule == NULL )
+ {
+ break;
+ }
+
+ rule->can_ifindex = ifindex;
+ rule->eth_ip = ip;
+ rule->eth_port = port;
+
+ hlist_add_head_rcu( &rule->list, &cegw_rule_eth_can );
+ break;
+ default:
+ pr_devel( "canethgw: unknown CEGW_CMD_INFO\n" );
+ break;
+ }
+
+ return 0;
}
-/***********************
- * module init/exit
- ***********************/
+static void cegw_flush( void )
+{
+ struct hlist_node* pos,* n;
+ struct cegw_rule* rule;
-static int __init cangw_init( void )
-{
+ hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_can_eth, list )
+ {
+ hlist_del( &rule->list );
+ kfree( rule );
+ }
+ hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_eth_can, list )
+ {
+ hlist_del( &rule->list );
+ kfree( rule );
+ }
+}
+
+static int cegw_remove_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
+{
+ struct rtmsg* r;
+ struct nlattr* tb[ CEGW_MAX+1 ];
+ int err = 0;
+
+ if( nlmsg_len(nlh) < sizeof(*r) )
+ return -EINVAL;
+
+ r = nlmsg_data( nlh );
+
+ if( r->rtm_family != AF_CAN )
+ return -EPFNOSUPPORT;
+
+ err = nlmsg_parse( nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL );
+ if( err != 0 )
+ return -EINVAL;
+
+ if( tb[CEGW_CMD_INFO] == NULL )
+ {
+ pr_devel( "canethgw: CEGW_CMD_INFO is missing in rtmsg\n" );
+ return -EINVAL;
+ }
+
+ if( *(int*)nla_data( tb[CEGW_CMD_INFO] ) != CEGW_FLUSH )
+ {
+ return -EINVAL;
+ }
+
+ cegw_flush();
+
+ return 0;
+}
+
+static int cegw_put_rule( struct sk_buff* skb, int type, struct cegw_rule* rule )
+{
+ int ifindex;
+ struct in_addr ip;
+ unsigned short port;
+ struct nlmsghdr* nlh;
+
+ ifindex = rule->can_ifindex;
+ ip = rule->eth_ip;
+ port = rule->eth_port;
+
+ nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
+ if( nlh == NULL )
+ return -EMSGSIZE;
+
+ /* type */
+ if( nla_put( skb, CEGW_TYPE, sizeof(type), &type ) < 0 )
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
+
+ /* can ifindex */
+ if( nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifindex), &ifindex ) < 0 )
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifindex) );
+
+ /* ip adress */
+ if( nla_put( skb, CEGW_ETH_IP, sizeof(ip), &ip) < 0 )
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ip) );
+
+ /* port */
+ if( nla_put( skb, CEGW_ETH_PORT, sizeof(port), &port ) < 0 )
+ goto cancel;
+ else
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(port) );
+
+ return skb->len;
+
+cancel:
+ nlmsg_cancel( skb, nlh );
+ return -EMSGSIZE;
+}
+
+static int cegw_dump_job( struct sk_buff* skb, struct netlink_callback* cb )
+{
+ struct cegw_rule* rule;
+ struct hlist_node* pos;
+ int idx = 0;
+ int s_idx = cb->args[0];
+
+ /* ToDo: skb max size */
+
+ rcu_read_lock();
+ hlist_for_each_entry_rcu( rule, pos, &cegw_rule_eth_can, list )
+ {
+ if( idx < s_idx )
+ goto cont1;
+
+ if( cegw_put_rule( skb, CEGW_RULE_ETH_CAN, rule ) < 0 )
+ goto brk;
+cont1:
+ idx++;
+ }
+ rcu_read_unlock();
+
+ rcu_read_lock();
+ hlist_for_each_entry_rcu( rule, pos, &cegw_rule_can_eth, list )
+ {
+ if( idx < s_idx )
+ goto cont2;
+
+ if( cegw_put_rule( skb, CEGW_RULE_CAN_ETH, rule ) < 0 )
+ goto brk;
+
+cont2:
+ idx++;
+ }
+ rcu_read_unlock();
+
+brk:
+ cb->args[0] = idx;
+
+ return skb->len;
+}
+
+/**
+ * cegw_thread_start - start working threads
+ * Two threads are started. One is serving udp->can routing and the other
+ * can->udp.
+ *
+ * @return 0 on success, -1 otherwise
+ */
+static int cegw_thread_start( void* data )
+{
struct sockaddr_in udp_addr;
struct sockaddr_can can_addr;
- int ifidx = 0;
+ struct cegw_setting* set;
+
+ set = (struct cegw_setting*)data;
+
+ can_addr.can_family = AF_CAN;
+ can_addr.can_ifindex = 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 )
+ udp_addr.sin_family = AF_INET;
+ udp_addr.sin_port = htons( set->eth_port );
+ udp_addr.sin_addr = set->eth_ip;
+
+ kfree( set );
+ mutex_lock( &cegw_mutex );
+ if( cegw_state == CEGW_EXIT )
+ return -1;
+ /* stops threads if exist */
+ cegw_thread_stop();
+
+ /* create and bind sockets */
+ if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock) != 0 )
{
- printk( KERN_ERR "error: vcan0 not found\n" );
+ printk( KERN_ERR "canethgw: udp socket creation failed\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" );
+ printk( KERN_ERR "canethgw: can socket 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 )
+
+ if( kernel_bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 )
{
- printk( KERN_ERR "can_sock bind failed\n" );
+ printk( KERN_ERR "canethgw: udp socket binding failed\n" );
+ sock_release( udp_sock );
+ sock_release( can_sock );
return -1;
}
-
- /* 2. create udp socket and bind to it */
- if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
+
+ if( kernel_bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
{
- printk( "error: udp_sock creation failed\n" );
+ printk( KERN_ERR "canethgw: can socket binding failed\n" );
+ kernel_sock_shutdown( udp_sock, SHUT_RDWR );
+ sock_release( udp_sock );
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 ?!? */
+ /* start threads */
+ cegw_state = CEGW_RUN;
+
+ eth_to_can = kthread_create( cegw_udp_can, NULL, "canethgw" );
+ if( IS_ERR( eth_to_can ) )
{
- printk( "error: binding failed\n" );
+ cegw_state = CEGW_STOP;
+ sock_release( udp_sock );
+ sock_release( can_sock );
+ return -1;
+ }
+ get_task_struct( eth_to_can );
+ wake_up_process( eth_to_can );
+
+ can_to_eth = kthread_create( cegw_can_udp, NULL, "canethgw" );
+ if( IS_ERR( can_to_eth ) )
+ {
+ cegw_state = CEGW_STOP;
+ kernel_sock_shutdown( udp_sock, SHUT_RDWR );
+ kthread_stop( eth_to_can );
sock_release( udp_sock );
sock_release( can_sock );
return -1;
}
+ /* ToDo: free this? */
+ get_task_struct( can_to_eth );
+ wake_up_process( can_to_eth );
- /* 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" );
- }
- */
-
+ mutex_unlock( &cegw_mutex );
+ pr_devel( "threads are running\n" );
return 0;
}
-static void __exit cangw_exit( void )
+/**
+ * cegw_thread_stop
+ * Waits for threads to stop. Does nothing if cegw_state == CEGW_STOP.
+ *
+ * @return 0
+ */
+static int cegw_thread_stop( void )
{
+ int how = SHUT_RDWR;
+ struct sock* sk = NULL;
+
+ if( cegw_state == CEGW_STOP )
+ return 0;
+
+ cegw_state = CEGW_STOP;
+ /* shut down socket */
+ sk = can_sock->sk;
+ how++;
+ lock_sock( sk );
+ sk->sk_shutdown |= how;
+ sk->sk_state_change( sk );
+ release_sock( sk );
+
+ kernel_sock_shutdown( udp_sock, SHUT_RDWR );
+
+ /* wait for return to reuse port if restart */
+ kthread_stop( eth_to_can );
+ kthread_stop( can_to_eth );
sock_release( udp_sock );
sock_release( can_sock );
-
- printk( "cangw: exit\n" );
- //kthread_stop( ts );
+ can_to_eth = NULL;
+ eth_to_can = NULL;
+
+ return 0;
}
-module_init( cangw_init );
-module_exit( cangw_exit );
+static int __init cegw_init( void )
+{
+ /* subscribe to netlink */
+ rtnl_register( PF_CAN, RTM_GETROUTE, NULL, cegw_dump_job, NULL );
+ rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_create_job, NULL, NULL );
+ rtnl_register( PF_CAN, RTM_DELROUTE, cegw_remove_job, NULL, NULL );
+
+ return 0;
+}
+
+static void __exit cegw_exit( void )
+{
+ /* ToDo: effect on cangw? */
+ rtnl_unregister_all( PF_CAN );
+
+ /* wait for rtnl callbacks */
+ rtnl_lock();
+ rtnl_unlock();
+
+ mutex_lock( &cegw_mutex );
+ cegw_thread_stop();
+ cegw_state = CEGW_EXIT;
+ mutex_unlock( &cegw_mutex );
+
+ cegw_flush();
+}
+
+module_init( cegw_init );
+module_exit( cegw_exit );
+
projects / can-eth-gw.git / blobdiff