3 #include <linux/module.h>
4 #include <linux/kernel.h>
5 #include <linux/kthread.h>
6 #include <linux/sched.h>
7 #include <linux/delay.h>
8 #include <linux/wait.h>
9 #include <linux/netdevice.h>
10 #include <linux/socket.h>
11 #include <linux/net.h>
12 #include <linux/can/core.h>
13 #include <linux/can.h>
14 #include <net/rtnetlink.h>
17 #include <linux/completion.h>
18 #include <linux/mutex.h>
19 #include <net/inet_common.h>
23 * [ ] encapsule module - check inputs
24 * [ ] refactor - chc .h
26 * [ ] rtnl vs nl functions
28 * [ ] change listening
29 * [ ] clean exit - threads, jobs
32 MODULE_LICENSE( "GPL" );
34 static int cegw_udp_can( void* data );
35 inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port );
36 static int cegw_can_udp( void* data );
37 inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex );
38 static void cegw_thread_start( void );
39 static void cegw_thread_stop( void );
40 static int cegw_thread_restart( void* arg );
42 #define CEGW_STOPPED 0
43 #define CEGW_RUNNING 1
45 static struct socket* can_sock, * udp_sock;
46 static struct task_struct* eth_to_can, * can_to_eth;
47 /* ToDo: protect with mutex */
48 static int cegw_state = CEGW_STOPPED;
53 struct in_addr dst_addr;
54 unsigned short dst_port;
55 struct hlist_node list;
61 struct hlist_node list;
64 HLIST_HEAD( can_eth_job );
65 HLIST_HEAD( eth_can_job );
69 struct can_filter filter;
72 struct in_addr eth_addr;
73 unsigned short eth_port;
76 DECLARE_COMPLETION( cegw_compl_init );
77 DECLARE_COMPLETION( cegw_compl_exit );
78 DEFINE_MUTEX( cegw_setting_mutex );
80 inline static void cegw_udp_send( struct socket* udp_sock, struct can_frame* cf, struct in_addr ipaddr, u16 port )
83 struct sockaddr_in addr;
86 addr.sin_family = AF_INET;
87 addr.sin_port = htons( port );
88 addr.sin_addr = ipaddr;
91 mh.msg_namelen = sizeof( addr );
92 mh.msg_control = NULL;
93 mh.msg_controllen = 0;
97 vec.iov_len = sizeof( *cf );
99 kernel_sendmsg( udp_sock, &mh, &vec, 1, sizeof( *cf ) );
102 inline static void cegw_can_send( struct socket* can_sock, struct can_frame* cf, int ifindex )
106 struct sockaddr_can addr;
108 addr.can_family = AF_CAN;
109 addr.can_ifindex = ifindex;
112 mh.msg_namelen = sizeof( addr );
113 mh.msg_control = NULL;
114 mh.msg_controllen = 0;
118 vec.iov_len = sizeof( *cf );
120 kernel_sendmsg( can_sock, &mh, &vec, 1, sizeof( *cf ) ); /* ToDo: handle error */
124 * cegw_udp_can - performs udp->can routing
125 * This function is run as a thread.
127 static int cegw_udp_can( void* data )
132 struct eth_can_gw* job;
133 struct hlist_node* pos;
136 struct sockaddr_in udp_addr;
137 struct sockaddr_can can_addr;
139 can_addr.can_family = AF_CAN;
140 can_addr.can_ifindex = 0;
142 udp_addr.sin_family = AF_INET;
143 mutex_lock( &cegw_setting_mutex );
144 udp_addr.sin_port = htons( cegw_setting.eth_port );
145 udp_addr.sin_addr = cegw_setting.eth_addr;
146 mutex_unlock( &cegw_setting_mutex );
152 mh.msg_control = NULL;
153 mh.msg_controllen = 0;
156 if( sock_create_kern( PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock) != 0 )
158 printk( KERN_ERR "canethgw: udp socket creation failed\n" );
162 if( sock_create_kern( PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0 )
164 printk( KERN_ERR "canethgw: can socket creation failed\n" );
168 if( kernel_bind( udp_sock, (struct sockaddr*)&udp_addr, sizeof( udp_addr ) ) != 0 )
170 printk( KERN_ERR "canethgw: udp socket binding failed\n" );
171 sock_release( udp_sock );
172 sock_release( can_sock );
176 if( kernel_bind( can_sock, (struct sockaddr*) &can_addr, sizeof(can_addr) ) != 0 )
178 printk( KERN_ERR "canethgw: can socket binding failed\n" );
179 sock_release( udp_sock );
180 sock_release( can_sock );
184 pr_devel( "canethgw: cegw_udp_can init complete\n" );
185 complete_all( &cegw_compl_init ); /* ToDo: why _all? */
186 pr_devel( "canethgw: cegw_udp_can is entering working cycle\n" );
190 if( cegw_state == CEGW_STOPPED )
193 vec.iov_len = sizeof(cf);
194 recv_size = kernel_recvmsg( udp_sock, &mh, &vec, 1, sizeof(cf), 0 ); /* ToDo: handle error, size check */
199 hlist_for_each_entry_rcu( job, pos, ð_can_job, list )
201 rcu_read_lock(); /**/
202 can_ifidx = job->dst_if_idx;
204 /* ToDo: from filter */
205 cegw_can_send( can_sock, &cf, can_ifidx );
209 pr_devel( "canethgw: cegw_udp_can is ready for termination\n" );
210 wait_for_completion( &cegw_compl_exit );
211 sock_release( udp_sock );
212 sock_release( can_sock );
213 cegw_state = CEGW_STOPPED; /* ToDo */
214 pr_devel( "canethgw: cegw_udp_can terminates\n" );
219 * cegw_can_udp - performs can->udp routing
221 static int cegw_can_udp( void* data )
226 struct sockaddr_can ca;
227 struct can_eth_gw* job;
228 struct hlist_node* pos;
229 struct in_addr eth_addr;
234 mh.msg_namelen = sizeof( ca );
235 mh.msg_control = NULL;
236 mh.msg_controllen = 0;
239 wait_for_completion( &cegw_compl_init );
240 pr_devel( "canethgw: cegw_can_udp is entering working cycle\n" );
244 if( cegw_state == CEGW_STOPPED ) /**/
247 vec.iov_len = sizeof( cf );
249 recv_size = kernel_recvmsg( can_sock, &mh, &vec, 1, sizeof( cf ), 0 );
254 hlist_for_each_entry_rcu( job, pos, &can_eth_job, list )
257 eth_addr = job->dst_addr;
258 eth_port = job->dst_port;
260 if( job->src_ifindex == ca.can_ifindex )
261 cegw_udp_send( udp_sock, &cf, eth_addr, eth_port );
265 complete_all( &cegw_compl_exit );
266 pr_devel( "canethgw: cegw_can_udp terminates\n" );
272 static int cegw_create_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
274 struct nlattr* tb[ CEGW_MAX+1 ];
275 struct can_eth_gw* cethgw = NULL;
276 struct eth_can_gw* ecangw = NULL;
283 if (nlmsg_len(nlh) < sizeof(*r))
287 err = nlmsg_parse( nlh, sizeof( struct rtmsg ), tb, CEGW_MAX, NULL );
290 printk( KERN_ERR "canethgw: nlmsg_parse error\n" );
294 if( tb[CEGW_CMD_INFO] == NULL )
296 printk( KERN_ERR "canethgw: bad cmd error\n" );
300 switch( *(int*)nla_data( tb[CEGW_CMD_INFO] ) )
303 if( cegw_state == CEGW_RUNNING )
305 mutex_lock( &cegw_setting_mutex );
306 cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
307 cegw_setting.eth_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
308 mutex_unlock( &cegw_setting_mutex );
309 kthread_run( cegw_thread_restart, NULL, "canethgw" );
312 mutex_lock( &cegw_setting_mutex );
313 cegw_setting.eth_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
314 cegw_setting.eth_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
315 mutex_unlock( &cegw_setting_mutex );
319 case CEGW_RULE_CAN_ETH: /* new can->eth rule */
321 ifindex = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] );
322 ip = *(u32*)nla_data( tb[CEGW_ETH_IP] );
323 port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
324 pr_devel( "canethgw: new can->eth rule - (%d)->(%x:%hu)\n", ifindex, ip, port );
326 cethgw = kmalloc( sizeof(struct can_eth_gw), GFP_KERNEL );
331 cethgw->src_ifindex = ifindex;
332 cethgw->dst_addr = *(struct in_addr*)nla_data( tb[CEGW_ETH_IP] );
333 cethgw->dst_port = *(u16*)nla_data( tb[CEGW_ETH_PORT] );
335 hlist_add_head_rcu( &cethgw->list, &can_eth_job );
338 case CEGW_RULE_ETH_CAN: /* new eth->can rule */
339 printk( KERN_INFO "can:%d\n", *(int*)nla_data( tb[CEGW_CAN_IFINDEX] ) );
340 ecangw = kmalloc( sizeof(struct eth_can_gw), GFP_KERNEL );
345 ecangw->dst_if_idx = *(int*)nla_data( tb[CEGW_CAN_IFINDEX] );
346 hlist_add_head_rcu( &ecangw->list, ð_can_job );
349 pr_devel( "canethgw: unknown CEGW_CMD_INFO\n" );
350 /* ToDo undef operation */
357 static int cegw_remove_job( struct sk_buff* skb, struct nlmsghdr* nlh, void* arg )
360 struct nlattr* tb[ CEGW_MAX+1 ];
361 struct hlist_node* pos,* n;
362 struct can_eth_gw* ceth;
363 struct eth_can_gw* ecan;
367 if( nlmsg_len(nlh) < sizeof(*r) )
370 r = nlmsg_data( nlh );
372 if( r->rtm_family != AF_CAN )
373 return -EPFNOSUPPORT;
376 if( r->gwtype != CGW_TYPE_CAN_ETH_UDP )
379 printk( "attrsize=%d\n", nlmsg_attrlen(nlh, sizeof(struct rtmsg)) );
381 err = nlmsg_parse( nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL );
385 if( tb[CEGW_CMD_INFO] == NULL )
388 if( *(int*)nla_data( tb[CEGW_CMD_INFO] ) == CEGW_FLUSH )
390 hlist_for_each_entry_safe( ceth, pos, n, &can_eth_job, list )
392 hlist_del( &ceth->list );
395 hlist_for_each_entry_safe( ecan, pos, n, ð_can_job, list )
397 hlist_del( &ecan->list );
405 static int cegw_dump_job( struct sk_buff* skb, struct netlink_callback* cb )
407 struct can_eth_gw* ceth;
408 struct eth_can_gw* ecan;
409 struct hlist_node* pos;
410 struct nlmsghdr* nlh;
412 int s_idx = cb->args[0];
414 struct in_addr dst_ip;
415 unsigned short dst_port;
418 hlist_for_each_entry_rcu( ecan, pos, ð_can_job, list )
424 nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
426 ifidx = ecan->dst_if_idx;
427 type = CEGW_RULE_ETH_CAN;
428 nla_put( skb, CEGW_TYPE, sizeof(type), &type );
429 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
431 nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifidx), &ifidx ); /* ToDo return */
432 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) );
439 hlist_for_each_entry_rcu( ceth, pos, &can_eth_job, list )
444 nlh = nlmsg_put( skb, 0, 0, 0, 0, 0 );
446 ifidx = ceth->src_ifindex;
447 type = CEGW_RULE_CAN_ETH;
448 dst_ip = ceth->dst_addr;
449 dst_port = ceth->dst_port;
451 nla_put( skb, CEGW_TYPE, sizeof(type), &type );
452 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(type) );
454 nla_put( skb, CEGW_CAN_IFINDEX, sizeof(ifidx), &ifidx ); /* ToDo return */
455 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(ifidx) );
457 nla_put( skb, CEGW_ETH_IP, sizeof(dst_ip), &dst_ip ); /* ToDo return */
458 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(dst_ip) );
460 nla_put( skb, CEGW_ETH_PORT, sizeof(dst_port), &dst_port ); /* ToDo return */
461 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN( sizeof(dst_port) );
463 //nla_put( skb, CGW_ETH_IP, sizeof() IP_ADDR )
469 /* ToDo nlmsg_cancel */
475 static void cegw_thread_start( void )
477 cegw_state = CEGW_RUNNING;
479 INIT_COMPLETION( cegw_compl_init );
480 INIT_COMPLETION( cegw_compl_exit );
482 eth_to_can = kthread_create( cegw_udp_can, NULL, "canethgw" );
483 if( !IS_ERR( eth_to_can ) )
485 get_task_struct( eth_to_can );
486 wake_up_process( eth_to_can );
488 can_to_eth = kthread_run( cegw_can_udp, NULL, "canethgw" );
489 if( !IS_ERR( can_to_eth ) )
491 get_task_struct( can_to_eth );
492 wake_up_process( can_to_eth );
495 /* ToDo: kthread creation fail */
496 printk( KERN_INFO "threads are running\n" );
499 /* ToDo: stop when no threads started */
500 static void cegw_thread_stop( void )
503 struct sock* sk = NULL;
505 /* be sure sockets exist */
506 wait_for_completion( &cegw_compl_init );
507 cegw_state = CEGW_STOPPED;
509 /* shut down socket */
513 sk->sk_shutdown |= how;
514 sk->sk_state_change( sk );
517 kernel_sock_shutdown( udp_sock, SHUT_RDWR );
519 /* wait for return to reuse port if restart */
520 kthread_stop( eth_to_can );
521 kthread_stop( can_to_eth );
525 * cegw_thread_restart
527 static int cegw_thread_restart( void* data )
529 printk( "restart\n" );
537 /***********************
539 ***********************/
541 static int __init cangw_init( void )
543 /* subscribe to netlink */
544 if( __rtnl_register( PF_CAN, RTM_GETROUTE, NULL, cegw_dump_job, NULL ) != 0 )
546 printk( KERN_ERR "error: rtnl_register fail\n" );
549 __rtnl_register( PF_CAN, RTM_NEWROUTE, cegw_create_job, NULL, NULL );
550 __rtnl_register( PF_CAN, RTM_DELROUTE, cegw_remove_job, NULL, NULL );
555 static void __exit cangw_exit( void )
557 if( cegw_state == CEGW_RUNNING )
560 /* ToDo: frees mem_cache? */
561 /* udp must not exists */
564 /* ToDo: unregister netlink
566 printk( "cangw: exit\n" );
569 module_init( cangw_init );
570 module_exit( cangw_exit );