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 void cegw_thread_start( void );
+static int cegw_thread_start( void );
static void cegw_thread_stop( void );
static int cegw_thread_restart( void* arg );
#define CEGW_STOPPED 0
#define CEGW_RUNNING 1
-static struct socket* can_sock, * udp_sock;
-static struct task_struct* eth_to_can, * can_to_eth;
+static struct socket* can_sock = NULL, * udp_sock = NULL;
+static struct task_struct* eth_to_can = NULL, * can_to_eth = NULL;
/* ToDo: protect with mutex */
static int cegw_state = CEGW_STOPPED;
-struct can_eth_gw
+struct cegw_rule
{
- int src_ifindex;
- struct in_addr dst_addr;
- unsigned short dst_port;
- struct hlist_node list;
-};
-
-struct eth_can_gw
-{
- int dst_if_idx;
+ int can_ifindex;
+ struct in_addr eth_ip;
+ unsigned short eth_port;
struct hlist_node list;
};
-HLIST_HEAD( can_eth_job );
-HLIST_HEAD( eth_can_job );
+HLIST_HEAD( cegw_rule_can_eth );
+HLIST_HEAD( cegw_rule_eth_can );
struct
{
- struct can_filter filter;
- int can_idx;
- /* bind on if */
+ int can_ifindex;
struct in_addr eth_addr;
unsigned short eth_port;
} cegw_setting;
-DECLARE_COMPLETION( cegw_compl_init );
-DECLARE_COMPLETION( cegw_compl_exit );
DEFINE_MUTEX( cegw_setting_mutex );
inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port )
struct can_frame cf;
struct kvec vec;
struct msghdr mh;
- struct eth_can_gw* job;
+ struct cegw_rule* job;
struct hlist_node* pos;
int can_ifidx;
int recv_size;
- struct sockaddr_in udp_addr;
- struct sockaddr_can can_addr;
-
- can_addr.can_family = AF_CAN;
- can_addr.can_ifindex = 0;
-
- udp_addr.sin_family = AF_INET;
- mutex_lock( &cegw_setting_mutex );
- udp_addr.sin_port = htons( cegw_setting.eth_port );
- udp_addr.sin_addr = cegw_setting.eth_addr;
- mutex_unlock( &cegw_setting_mutex );
mh.msg_name = NULL;
mh.msg_namelen = 0;
mh.msg_controllen = 0;
mh.msg_flags = 0;
- if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock) != 0 )
- {
- printk( KERN_ERR "canethgw: udp socket creation failed\n" );
- return -1;
- }
-
- if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
- {
- printk( KERN_ERR "canethgw: can socket creation failed\n" );
- return -1;
- }
-
- if( kernel_bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 )
- {
- printk( KERN_ERR "canethgw: udp socket binding failed\n" );
- sock_release( udp_sock );
- sock_release( can_sock );
- return -1;
- }
-
- if( kernel_bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
- {
- printk( KERN_ERR "canethgw: can socket binding failed\n" );
- sock_release( udp_sock );
- sock_release( can_sock );
- return -1;
- }
-
- pr_devel( "canethgw: cegw_udp_can init complete\n" );
- complete_all( &cegw_compl_init ); /* ToDo: why _all? */
- pr_devel( "canethgw: cegw_udp_can is entering working cycle\n" );
-
while( 1 )
{
if( cegw_state == CEGW_STOPPED )
{
continue;
}
- hlist_for_each_entry_rcu( job, pos, ð_can_job, list )
+ hlist_for_each_entry_rcu( job, pos, &cegw_rule_eth_can, list )
{
rcu_read_lock(); /**/
- can_ifidx = job->dst_if_idx;
+ can_ifidx = job->can_ifindex;
rcu_read_unlock();
/* ToDo: from filter */
cegw_can_send( can_sock, &cf, can_ifidx );
}
}
- pr_devel( "canethgw: cegw_udp_can is ready for termination\n" );
- wait_for_completion( &cegw_compl_exit );
- sock_release( udp_sock );
- sock_release( can_sock );
- cegw_state = CEGW_STOPPED; /* ToDo */
- pr_devel( "canethgw: cegw_udp_can terminates\n" );
return 0;
}
struct kvec vec;
struct can_frame cf;
struct sockaddr_can ca;
- struct can_eth_gw* job;
+ struct cegw_rule* rule;
struct hlist_node* pos;
struct in_addr eth_addr;
u16 eth_port;
mh.msg_controllen = 0;
mh.msg_flags = 0;
- wait_for_completion( &cegw_compl_init );
- pr_devel( "canethgw: cegw_can_udp is entering working cycle\n" );
-
while( 1 )
{
if( cegw_state == CEGW_STOPPED ) /**/
{
continue;
}
- hlist_for_each_entry_rcu( job, pos, &can_eth_job, list )
+ hlist_for_each_entry_rcu( rule, pos, &cegw_rule_can_eth, list )
{
rcu_read_lock();
- eth_addr = job->dst_addr;
- eth_port = job->dst_port;
+ eth_addr = rule->eth_ip;
+ eth_port = rule->eth_port;
rcu_read_unlock();
- if( job->src_ifindex == ca.can_ifindex )
+ if( rule->can_ifindex == ca.can_ifindex )
cegw_udp_send( udp_sock, &cf, eth_addr, eth_port );
}
}
- complete_all( &cegw_compl_exit );
- pr_devel( "canethgw: cegw_can_udp terminates\n" );
return 0;
}
static int cegw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
{
struct nlattr* tb[ CEGW_MAX+1 ];
- struct can_eth_gw* cethgw = NULL;
- struct eth_can_gw* ecangw = NULL;
+ struct cegw_rule* rule = NULL;
int ifindex;
- u32 ip;
- u16 port;
+ struct rtmsg* r;
+ struct in_addr ip;
+ unsigned short port;
int err = 0;
- /* ToDo: size check
- if (nlmsg_len(nlh) < sizeof(*r))
+ if( nlmsg_len(nlh) < sizeof(*r) )
return -EINVAL;
- */
- err = nlmsg_parse( nlh, sizeof( struct rtmsg ), tb, CEGW_MAX, NULL );
+ 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 )
{
- printk( KERN_ERR "canethgw: nlmsg_parse error\n" );
+ pr_devel( "canethgw: nlmsg_parse error\n" );
return err;
}
if( tb[CEGW_CMD_INFO] == NULL )
{
- printk( KERN_ERR "canethgw: bad cmd error\n" );
+ 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( cegw_state == CEGW_RUNNING )
- {
- mutex_lock( &cegw_setting_mutex );
- cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
- cegw_setting.eth_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
- mutex_unlock( &cegw_setting_mutex );
- kthread_run( cegw_thread_restart, NULL, "canethgw" );
- } else
+ if( !tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] )
{
- mutex_lock( &cegw_setting_mutex );
- cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
- cegw_setting.eth_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
- mutex_unlock( &cegw_setting_mutex );
- cegw_thread_start();
+ pr_devel( "canethgw: missing attribute for CEGW_LISTEN\n" );
+ return -EINVAL;
}
+
+ mutex_lock( &cegw_setting_mutex );
+ cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
+ cegw_setting.eth_port = *(unsigned short*)nla_data( tb[CEGW_ETH_PORT] );
+ kthread_run( cegw_thread_restart, NULL, "canethgw" );
+ mutex_unlock( &cegw_setting_mutex );
break;
- case CEGW_RULE_CAN_ETH: /* new can->eth rule */
- {
+ 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 = *(u32*)nla_data( tb[CEGW_ETH_IP] );
- port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
- pr_devel( "canethgw: new can->eth rule - (%d)->(%x:%hu)\n", ifindex, ip, port );
+ 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 );
- cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL );
- if( cethgw == NULL )
+ rule = kmalloc( sizeof(struct cegw_rule), GFP_KERNEL );
+ if( rule == NULL )
{
break;
}
- cethgw->src_ifindex = ifindex;
- cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
- cethgw->dst_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
- hlist_add_head_rcu( &cethgw->list, &can_eth_job );
- }
- break;
- case CEGW_RULE_ETH_CAN: /* new eth->can rule */
- printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CEGW_CAN_IFINDEX] ) );
- ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL );
- if( ecangw == NULL )
+ 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;
}
- ecangw->dst_if_idx = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] );
- hlist_add_head_rcu( &ecangw->list, ð_can_job );
+
+ 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" );
- /* ToDo undef operation */
break;
}
struct rtmsg* r;
struct nlattr* tb[ CEGW_MAX+1 ];
struct hlist_node* pos,* n;
- struct can_eth_gw* ceth;
- struct eth_can_gw* ecan;
+ struct cegw_rule* rule;
int err = 0;
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, 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 )
+ if( *(int*)nla_data( tb[CEGW_CMD_INFO] ) != CEGW_FLUSH )
{
- 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 );
- }
+ return -EINVAL;
+ }
+
+ hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_can_eth, list )
+ {
+ hlist_del( &rule->list );
+ kfree( rule );
}
- // tb[]
+ hlist_for_each_entry_safe( rule, pos, n, &cegw_rule_eth_can, list )
+ {
+ hlist_del( &rule->list );
+ kfree( rule );
+ }
+
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 can_eth_gw* ceth;
- struct eth_can_gw* ecan;
+ struct cegw_rule* rule;
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;
+
+ /* ToDo: skb max size */
rcu_read_lock();
- hlist_for_each_entry_rcu( ecan, pos, ð_can_job, list )
+ hlist_for_each_entry_rcu( rule, pos, &cegw_rule_eth_can, list )
{
+ if( idx < s_idx )
+ goto cont1;
- // if( idx < s_idx )
- // goto cont1;
-
- nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
-
- ifidx = ecan->dst_if_idx;
- type = CEGW_RULE_ETH_CAN;
- nla_put( skb, CEGW_TYPE, sizeof(type), &type );
- nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
-
- nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifidx), &ifidx ); /* ToDo return */
- nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) );
+ 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( ceth, pos, &can_eth_job, list )
+ hlist_for_each_entry_rcu( rule, pos, &cegw_rule_can_eth, list )
{
- // if( idx < s_idx )
- // goto cont2;
-
- nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
-
- ifidx = ceth->src_ifindex;
- type = CEGW_RULE_CAN_ETH;
- dst_ip = ceth->dst_addr;
- dst_port = ceth->dst_port;
+ if( idx < s_idx )
+ goto cont2;
- nla_put( skb, CEGW_TYPE, sizeof(type), &type );
- nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
-
- nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifidx), &ifidx ); /* ToDo return */
- nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) );
-
- nla_put( skb, CEGW_ETH_IP, sizeof(dst_ip), &dst_ip ); /* ToDo return */
- nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(dst_ip) );
+ if( cegw_put_rule( skb, CEGW_RULE_CAN_ETH, rule ) < 0 )
+ goto brk;
- nla_put( skb, CEGW_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 */
+brk:
cb->args[0] = idx;
return skb->len;
}
-static void cegw_thread_start( void )
+static int cegw_thread_start( void )
{
- cegw_state = CEGW_RUNNING;
+ struct sockaddr_in udp_addr;
+ struct sockaddr_can can_addr;
- INIT_COMPLETION( cegw_compl_init );
- INIT_COMPLETION( cegw_compl_exit );
+ can_addr.can_family = AF_CAN;
+ can_addr.can_ifindex = 0;
+ udp_addr.sin_family = AF_INET;
+ udp_addr.sin_port = htons( cegw_setting.eth_port );
+ udp_addr.sin_addr = cegw_setting.eth_addr;
+
+ /* create and bind sockets */
+ if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock) != 0 )
+ {
+ printk( KERN_ERR "canethgw: udp socket creation failed\n" );
+ return -1;
+ }
+
+ if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
+ {
+ printk( KERN_ERR "canethgw: can socket creation failed\n" );
+ return -1;
+ }
+
+ if( kernel_bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 )
+ {
+ printk( KERN_ERR "canethgw: udp socket binding failed\n" );
+ sock_release( udp_sock );
+ sock_release( can_sock );
+ return -1; /* ToDo: state==RUNNING and return? */
+ }
+
+ if( kernel_bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
+ {
+ printk( KERN_ERR "canethgw: can socket binding failed\n" );
+ sock_release( udp_sock );
+ sock_release( can_sock );
+ return -1;
+ }
+
+ /* start threads */
+ cegw_state = CEGW_RUNNING;
eth_to_can = kthread_create( cegw_udp_can, NULL, "canethgw" );
if( !IS_ERR( eth_to_can ) )
{
get_task_struct( eth_to_can );
wake_up_process( eth_to_can );
}
- can_to_eth = kthread_run( cegw_can_udp, NULL, "canethgw" );
+ can_to_eth = kthread_create( cegw_can_udp, NULL, "canethgw" );
if( !IS_ERR( can_to_eth ) )
{
+ /* ToDo: free this? */
get_task_struct( can_to_eth );
wake_up_process( can_to_eth );
}
/* ToDo: kthread creation fail */
- printk( KERN_INFO "threads are running\n" );
+ pr_devel( "threads are running\n" );
+
+ return 0;
}
-/* ToDo: stop when no threads started */
static void cegw_thread_stop( void )
{
int how = SHUT_RDWR;
struct sock* sk = NULL;
- /* be sure sockets exist */
- wait_for_completion( &cegw_compl_init );
+ /* no threads are running */
+ if( !can_to_eth && !eth_to_can )
+ return;
+
cegw_state = CEGW_STOPPED;
-
/* shut down socket */
sk = can_sock->sk;
how++;
/* 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 );
+ can_to_eth = NULL;
+ eth_to_can = NULL;
}
/**
*/
static int cegw_thread_restart( void* data )
{
- printk( "restart\n" );
-
cegw_thread_stop();
cegw_thread_start();
* module init/exit
***********************/
-static int __init cangw_init( void )
+static int __init cegw_init( void )
{
/* subscribe to netlink */
- if( __rtnl_register( PF_CAN, RTM_GETROUTE, NULL, cegw_dump_job, NULL ) != 0 )
- {
- printk( KERN_ERR "error: rtnl_register fail\n" );
- 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_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 cangw_exit( void )
+static void __exit cegw_exit( void )
{
- if( cegw_state == CEGW_RUNNING )
- {
- cegw_thread_stop();
- /* ToDo: frees mem_cache? */
- /* udp must not exists */
- }
+ /* ToDo: effect on others? */
+ rtnl_unregister_all( PF_CAN );
+
+ /* ToDo: no rtnl pending? */
+ cegw_thread_stop();
+ /* ToDo: frees mem_cache? */
+ /* udp must not exists */
- /* ToDo: unregister netlink
- * free jobs */
printk( "cangw: exit\n" );
}
-module_init( cangw_init );
-module_exit( cangw_exit );
+module_init( cegw_init );
+module_exit( cegw_exit );
projects / can-eth-gw.git / commitdiff