-#include "gw.h" /* override */
-
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/kthread.h>
#include <net/rtnetlink.h>
#include "canethgw.h"
+/**
+ * ToDo:
+ * [ ] encapsule module - check inputs
+ * [ ] refactor - chc .h
+ * [ ] dump callback
+ * [ ] rtnl vs nl functions
+ * [ ] stop threads
+ * [ ] change listening
+ * [ ] clean exit - threads, jobs
+ */
+
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;
+static int gw_udp_recv( void* data );
+static void gw_udp_send( struct can_frame* cf, struct in_addr ipaddr, u16 port );
+static int gw_can_recv( void* data );
+static void gw_can_send( struct can_frame* cf, int ifidx );
+static int listen( int can_ifidx, struct in_addr eth_addr, u16 eth_port );
-struct can_can_gw {
- struct can_filter filter;
- int src_idx;
- int dst_idx;
-};
+#define CEGW_STOPPED 0
+#define CEGW_RUNNING 1
+
+static struct task_struct* eth_to_can, * can_to_eth;
+static struct socket* udp_sock = NULL;
+static struct socket* can_sock = NULL;
+static int gw_state = CEGW_STOPPED;
struct can_eth_gw
{
- int src_if_index;
+ int src_if_idx;
struct in_addr dst_addr;
unsigned short dst_port;
+ struct hlist_node list;
};
struct eth_can_gw
{
- int dst_if;
+ int dst_if_idx;
+ struct hlist_node list;
};
+HLIST_HEAD( can_eth_job );
+HLIST_HEAD( eth_can_job );
+
struct cegw_setting
{
struct can_filter filter;
unsigned short dst_port;
};
-/* list entry for CAN gateways jobs */
-struct cgw_job {
- struct hlist_node list;
- struct rcu_head rcu;
- u32 handled_frames;
- u32 dropped_frames;
- union {
- /* CAN frame data source */
- struct net_device *dev;
- } src;
- union {
- /* CAN frame data destination */
- struct net_device *dev;
- } dst;
- union {
- struct can_can_gw ccgw;
- /* tbc */
- };
- u8 gwtype;
- u16 flags;
-};
/***********************
* UDP
***********************/
struct can_frame cf;
struct kvec vec;
struct msghdr mh;
+ struct eth_can_gw* job;
+ struct hlist_node* pos;
+ int can_ifidx;
vec.iov_base = &cf;
vec.iov_len = sizeof(cf);
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 );
+ hlist_for_each_entry_rcu( job, pos, ð_can_job, list )
+ {
+ rcu_read_lock(); /**/
+ can_ifidx = job->dst_if_idx;
+ rcu_read_unlock();
+ /* ToDo from filter */
+ gw_can_send( &cf, can_ifidx );
+ }
}
return 0;
}
-static void gw_udp_send( struct can_frame* cf )
+inline static void gw_udp_send( 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 );
struct msghdr mh;
struct kvec vec;
struct can_frame cf;
+ struct sockaddr_can ca;
+ struct can_eth_gw* job;
+ struct hlist_node* pos;
+ struct in_addr eth_addr;
+ u16 eth_port;
- mh.msg_name = NULL;
- mh.msg_namelen = 0;
+ mh.msg_name = &ca;
+ mh.msg_namelen = sizeof( ca );
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 );
+ printk( "received can msg_id:%d, from:%d\n", cf.can_id, ca.can_ifindex );
+ hlist_for_each_entry_rcu( job, pos, &can_eth_job, list )
+ {
+ rcu_read_lock();
+ eth_addr = job->dst_addr;
+ eth_port = job->dst_port;
+ rcu_read_unlock();
+ printk( KERN_INFO "%x\n", eth_addr.s_addr );
+ if( job->src_if_idx == ca.can_ifindex )
+ gw_udp_send( &cf, eth_addr, eth_port );
+ }
}
return 0;
}
-static void gw_can_send( struct can_frame* cf )
+inline static void gw_can_send( struct can_frame* cf, int ifidx )
{
struct msghdr mh;
- struct kvec vec;
+ struct kvec vec;
+ struct sockaddr_can ca =
+ {
+ .can_family = AF_CAN,
+ .can_ifindex = ifidx
+ };
- mh.msg_name = NULL;
- mh.msg_namelen = 0;
+ mh.msg_name = &ca;
+ mh.msg_namelen = sizeof( ca );
mh.msg_control = NULL;
mh.msg_controllen = 0;
mh.msg_flags = 0;
kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) );
}
-/**/
+/* NetLink */
-static int cgw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh,
- void* arg )
+static int cegw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
{
struct nlattr* tb[ CGW_MAX+1 ];
- struct rtcanmsg *r;
- struct cgw_job *gwj;
+ struct can_eth_gw* cethgw = NULL;
+ struct eth_can_gw* ecangw = NULL;
int err = 0;
+ /* ToDo: size check
if (nlmsg_len(nlh) < sizeof(*r))
return -EINVAL;
-
- r = nlmsg_data(nlh);
- if (r->can_family != AF_CAN)
- return -EPFNOSUPPORT;
-
- /* so far we only support CAN -> CAN routings */
- if (r->gwtype == CGW_TYPE_CAN_ETH_UDP)
- printk( KERN_INFO "canethgw: TYPE_CAN_ETH" );
- else
- return -1;
-
- err = nlmsg_parse( nlh, sizeof( struct rtcanmsg ), tb, CGW_MAX, NULL );
+ */
+
+ err = nlmsg_parse( nlh, sizeof( struct rtmsg ), tb, CGW_MAX, NULL );
if( err < 0 )
+ {
+ printk( KERN_ERR "error: nlmsg_parse\n" );
return err;
+ }
+
+ if( tb[CGW_CMD_INFO] == NULL )
+ {
+ printk( "error: bad cmd\n" );
+ return -EINVAL;
+ }
- printk( KERN_INFO "can:%d\n", *(u16*)nla_data( tb[CGW_CAN_IF] ) );
- printk( KERN_INFO "eth addr:%x\n", *(u16*)nla_data( tb[CGW_ETH_IP] ) );
- printk( KERN_INFO "eth port:%hu\n", *(u16*)nla_data( tb[CGW_ETH_PORT] ) );
+ switch( *(int*)nla_data( tb[CGW_CMD_INFO] ) )
+ {
+ case CEGW_LISTEN:
+ listen( 0, *(struct in_addr*)nla_data( tb[CGW_LISTEN_IP] ),
+ *(u16*)nla_data( tb[CGW_LISTEN_PORT] ) );
+ break;
+ case CGW_TYPE_CAN_ETH_UDP:
+ printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
+ printk( KERN_INFO "eth addr:%x\n", *(u32*)nla_data( tb[CGW_ETH_IP] ) );
+ printk( KERN_INFO "eth port:%hu\n", *(u16*)nla_data( tb[CGW_ETH_PORT] ) );
+ cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL );
+ if( cethgw == NULL )
+ {
+ printk( KERN_ERR "error: kmalloc\n" );
+ break;
+ }
+ cethgw->src_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
+ cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CGW_ETH_IP] );
+ cethgw->dst_port = *(u16*)nla_data( tb[CGW_ETH_PORT] );
+
+ hlist_add_head_rcu( &cethgw->list, &can_eth_job );
+ break;
+ case CGW_TYPE_ETH_CAN_UDP:
+ printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CGW_CAN_IF] ) );
+ ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL );
+ if( ecangw == NULL )
+ {
+ printk( KERN_ERR "error: kmalloc\n" );
+ break;
+ }
+ ecangw->dst_if_idx = *(int*)nla_data( tb[CGW_CAN_IF] );
+ hlist_add_head_rcu( &ecangw->list, ð_can_job );
+ break;
+ default:
+ printk( "default" );
+ /* ToDo undef operation */
+ break;
+ }
return 0;
}
-/***********************
- * module init/exit
- ***********************/
+static int cegw_remove_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
+{
+ struct rtmsg* r;
+ struct nlattr* tb[ CGW_MAX+1 ];
+ struct hlist_node* pos,* n;
+ struct can_eth_gw* ceth;
+ struct eth_can_gw* ecan;
-static int __init cangw_init( void )
-{
- struct sockaddr_in udp_addr;
- struct sockaddr_can can_addr;
- int ifidx = 0;
+ int err = 0;
+
+ if( nlmsg_len(nlh) < sizeof(*r) )
+ return -EINVAL;
- /* 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 )
+ r = nlmsg_data( nlh );
+
+ if( r->rtm_family != AF_CAN )
+ return -EPFNOSUPPORT;
+
+ /*
+ if( r->gwtype != CGW_TYPE_CAN_ETH_UDP )
+ return -EINVAL;
+ */
+ printk( "attrsize=%d\n", nlmsg_attrlen(nlh, sizeof(struct rtmsg)) );
+
+ err = nlmsg_parse( nlh, sizeof(struct rtmsg), tb, CGW_MAX, NULL );
+ if( err != 0 )
+ return -EINVAL;
+
+ if( tb[CGW_CMD_INFO] == NULL )
+ return -EINVAL;
+
+ if( *(int*)nla_data( tb[CGW_CMD_INFO] ) == CEGW_FLUSH )
{
- printk( KERN_ERR "error: vcan0 not found\n" );
- return -1;
+ hlist_for_each_entry_safe( ceth, pos, n, &can_eth_job, list )
+ {
+ hlist_del( &ceth->list );
+ kfree( ceth );
+ }
+ hlist_for_each_entry_safe( ecan, pos, n, ð_can_job, list )
+ {
+ hlist_del( &ecan->list );
+ kfree( ecan );
+ }
}
- ifidx = can_dev->ifindex;
- dev_put( can_dev );
+ // tb[]
+ return 0;
+}
+
+static int cegw_dump_job( struct sk_buff* skb, struct netlink_callback* cb )
+{
+ struct can_eth_gw* ceth;
+ struct eth_can_gw* ecan;
+ struct hlist_node* pos;
+ struct nlmsghdr* nlh;
+ int idx = 0;
+ int s_idx = cb->args[0];
+ int ifidx, type;
+ struct in_addr dst_ip;
+ unsigned short dst_port;
+
+ rcu_read_lock();
+ hlist_for_each_entry_rcu( ecan, pos, ð_can_job, list )
+ {
+
+ // if( idx < s_idx )
+ // goto cont1;
+
+ nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
+
+ ifidx = ecan->dst_if_idx;
+ type = CGW_TYPE_ETH_CAN_UDP;
+ nla_put( skb, CGW_TYPE, sizeof(type), &type );
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
+
+ nla_put( skb, CGW_CAN_IF, sizeof(ifidx), &ifidx ); /* ToDo return */
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) );
+cont1:
+ idx++;
+ }
+ rcu_read_unlock();
+
+ rcu_read_lock();
+ hlist_for_each_entry_rcu( ceth, pos, &can_eth_job, list )
+ {
+ // if( idx < s_idx )
+ // goto cont2;
- if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
+ nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
+
+ ifidx = ceth->src_if_idx;
+ type = CGW_TYPE_CAN_ETH_UDP;
+ dst_ip = ceth->dst_addr;
+ dst_port = ceth->dst_port;
+
+ nla_put( skb, CGW_TYPE, sizeof(type), &type );
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
+
+ nla_put( skb, CGW_CAN_IF, sizeof(ifidx), &ifidx ); /* ToDo return */
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) );
+
+ nla_put( skb, CGW_ETH_IP, sizeof(dst_ip), &dst_ip ); /* ToDo return */
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(dst_ip) );
+
+ nla_put( skb, CGW_ETH_PORT, sizeof(dst_port), &dst_port ); /* ToDo return */
+ nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(dst_port) );
+
+ //nla_put( skb, CGW_ETH_IP, sizeof() IP_ADDR )
+cont2:
+ idx++;
+ }
+ rcu_read_unlock();
+
+ /* ToDo nlmsg_cancel */
+ cb->args[0] = idx;
+
+ return skb->len;
+}
+
+static int listen( int can_ifidx, struct in_addr eth_addr, u16 eth_port )
+{
+ struct sockaddr_in udp_addr;
+ struct sockaddr_can can_addr;
+ struct socket* tmp;
+
+ printk( KERN_INFO "listen called\n" );
+
+ if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &tmp) != 0 )
{
printk( KERN_ERR "error: can_sock creation failed\n" );
return -1;
}
-
+
can_addr.can_family = AF_CAN;
- can_addr.can_ifindex = ifidx;
+ can_addr.can_ifindex = can_ifidx;
if( can_sock->ops->bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
{
return -1;
}
- /* 2. create udp socket and bind to it */
- if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
+ printk( KERN_INFO "can socket success\n" );
+
+ udp_addr.sin_family = AF_INET;
+ udp_addr.sin_port = htons( eth_port );
+ udp_addr.sin_addr = eth_addr;
+
+ printk( KERN_INFO "trying to bind\n" );
+ if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
{
- printk( "error: udp_sock creation failed\n" );
+ printk( "error: binding failed\n" );
+ sock_release( udp_sock );
sock_release( can_sock );
return -1;
}
+
+ printk( KERN_INFO "socket established\n" );
- udp_addr.sin_family = AF_INET;
- udp_addr.sin_port = htons( 10501 );
- udp_addr.sin_addr.s_addr = INADDR_ANY;
+ /* run threads */
+ eth_to_can = kthread_run( gw_udp_recv, NULL, "ethcangw" );
+ can_to_eth = kthread_run( gw_can_recv, NULL, "canethgw" );
- if( udp_sock->ops->bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 ) /* ref impl ?!? */
+ printk( KERN_INFO "threads are running\n" );
+
+ gw_state = CEGW_RUNNING;
+
+ return 0;
+}
+
+/***********************
+ * module init/exit
+ ***********************/
+
+static int __init cangw_init( void )
+{
+ if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
{
- printk( "error: binding failed\n" );
- sock_release( udp_sock );
+ printk( KERN_ERR "error: can_sock creation failed\n" );
+ return -1;
+ }
+
+ if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock ) != 0 )
+ {
+ printk( KERN_ERR "error: udp_sock creation failed\n" );
sock_release( can_sock );
return -1;
}
- /* 3. subscribe to netlink */
- //if( __rtnl_register( PF_CAN, RTM_GETROUTE, ) != 0 )
- __rtnl_register( PF_CAN, RTM_NEWROUTE, cgw_create_job, NULL, NULL );
+ /* subscribe to netlink */
+ if( __rtnl_register( PF_CAN, RTM_GETROUTE, NULL, cegw_dump_job, NULL ) != 0 )
+ {
+ printk( KERN_ERR "error: rtnl_register fail\n" );
+ sock_release( udp_sock );
+ sock_release( can_sock );
+ return -1;
+ }
+ __rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_create_job, NULL, NULL );
+ __rtnl_register( PF_CAN, RTM_DELROUTE, cegw_remove_job, NULL, NULL );
//__rtnl_register( PF_CAN, RTM_DELROUTE, )
- /* 4. run 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
static void __exit cangw_exit( void )
{
- sock_release( udp_sock );
- sock_release( can_sock );
-
+ if( gw_state == CEGW_RUNNING )
+ {
+ sock_release( udp_sock );
+ sock_release( can_sock );
+ /* ToDo: stop threads */
+ }
+
+ /* ToDo: unregister netlink
+ * free jobs */
printk( "cangw: exit\n" );
//kthread_stop( ts );
}
module_init( cangw_init );
module_exit( cangw_exit );
+
projects / can-eth-gw.git / blobdiff