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/if_arp.h>
12 #include <linux/net.h>
13 #include <linux/netdevice.h>
14 #include <linux/can/core.h>
15 #include <linux/can.h>
16 #include <net/rtnetlink.h>
19 #include <linux/completion.h>
20 #include <linux/mutex.h>
21 #include <net/inet_common.h>
23 MODULE_LICENSE("GPL");
25 static int cegw_udp2can(void *data);
26 static void cegw_udp_send(struct socket *udp_sock, struct can_frame *cf,
27 struct in_addr ipaddr, u16 port);
28 static int cegw_can2udp(void *data);
29 static void cegw_can_send(struct socket *can_sock, struct can_frame *cf,
31 static int cegw_thread_start(void *data);
32 static int cegw_thread_stop(void);
34 static int cegw_thread_recv_udp(void *data);
35 static int cegw_thread_recv_can(void *data);
45 struct in_addr eth_ip;
46 unsigned short eth_port;
47 struct hlist_node list;
51 struct in_addr eth_ip;
52 unsigned short eth_port;
56 static int cegw_state = CEGW_STOP;
57 static struct socket *can_sock = NULL, *udp_sock = NULL;
58 static struct socket *s_can = NULL, *s_eth = NULL;
59 static struct task_struct *eth_to_can = NULL, *can_to_eth = NULL;
60 static struct notifier_block notifier;
62 static HLIST_HEAD(rule_eth_can);
63 static HLIST_HEAD(rule_can_eth);
64 static DEFINE_MUTEX(rule_eth_can_mutex);
65 static DEFINE_MUTEX(rule_can_eth_mutex);
66 static DEFINE_MUTEX(cegw_mutex);
68 static void cegw_udp_send(struct socket *udp_sock, struct can_frame *cf,
69 struct in_addr ipaddr, u16 port)
72 struct sockaddr_in addr;
75 addr.sin_family = AF_INET;
76 addr.sin_port = htons(port);
77 addr.sin_addr = ipaddr;
80 mh.msg_namelen = sizeof(addr);
81 mh.msg_control = NULL;
82 mh.msg_controllen = 0;
86 vec.iov_len = sizeof(*cf);
88 /* FIXME: Convert endianing of cf->can_id */
89 kernel_sendmsg(udp_sock, &mh, &vec, 1, sizeof(*cf));
92 static void cegw_can_send(struct socket* can_sock, struct can_frame* cf,
97 struct sockaddr_can addr;
99 addr.can_family = AF_CAN;
100 addr.can_ifindex = ifindex;
103 mh.msg_namelen = sizeof(addr);
104 mh.msg_control = NULL;
105 mh.msg_controllen = 0;
109 vec.iov_len = sizeof(*cf);
111 kernel_sendmsg(can_sock, &mh, &vec, 1, sizeof(*cf));
115 * cegw_udp2can - performs udp->can routing
117 * This function is run as a thread.
119 static int cegw_udp2can(void *data)
124 struct cegw_rule* rule;
125 struct hlist_node* pos;
129 memset(&mh, 0, sizeof(mh));
131 while (cegw_state != CEGW_STOP) {
133 vec.iov_len = sizeof(cf);
134 recv_size = kernel_recvmsg(udp_sock, &mh, &vec, 1,
136 /* recv_size == 0 when shutting down */
137 if (recv_size != sizeof(cf) || recv_size == 0)
139 else if (recv_size < 0)
142 /* FIXME: Convert endianing of cf.can_id */
143 mutex_lock(&rule_eth_can_mutex);
144 hlist_for_each_entry(rule, pos, &rule_eth_can, list) {
145 can_ifidx = rule->can_ifindex;
146 /* ToDo: from filter */
147 cegw_can_send(can_sock, &cf, can_ifidx);
149 mutex_unlock(&rule_eth_can_mutex);
156 * cegw_can2udp - performs can->udp routing
160 static int cegw_can2udp(void* data)
165 struct sockaddr_can ca;
166 struct cegw_rule* rule;
167 struct hlist_node* pos;
168 struct in_addr eth_ip;
173 mh.msg_namelen = sizeof(ca);
174 mh.msg_control = NULL;
175 mh.msg_controllen = 0;
178 while (cegw_state != CEGW_STOP) {
180 vec.iov_len = sizeof(cf);
182 recv_size = kernel_recvmsg(can_sock, &mh, &vec, 1,
184 if (recv_size != sizeof(cf) || recv_size == 0)
186 else if (recv_size < 0)
189 mutex_lock(&rule_can_eth_mutex);
190 hlist_for_each_entry(rule, pos, &rule_can_eth, list) {
191 eth_ip = rule->eth_ip;
192 eth_port = rule->eth_port;
193 if (rule->can_ifindex == ca.can_ifindex)
194 cegw_udp_send(udp_sock, &cf, eth_ip, eth_port);
196 mutex_unlock(&rule_can_eth_mutex);
202 static int cegw_newroute(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
204 struct nlattr* tb[ CEGW_MAX+1 ];
205 struct cegw_rule* rule = NULL;
210 struct cegw_setting* set;
214 if (nlmsg_len(nlh) < sizeof(*r))
219 if (r->rtm_family != AF_CAN)
220 return -EPFNOSUPPORT;
222 err = nlmsg_parse(nlh, sizeof(*r), tb, CEGW_MAX, NULL);
224 pr_devel("canethgw: nlmsg_parse error\n");
228 fd_eth = *(int*)nla_data(tb[CEGW_ETH_SOCK]);
229 fd_can = *(int*)nla_data(tb[CEGW_CAN_SOCK]);
231 printk( "cegw: sock=%i,%i\n", fd_eth, fd_can );
233 s_can = sockfd_lookup( fd_can, &err );
234 s_eth = sockfd_lookup( fd_eth, &err );
236 if( s_can == NULL || s_eth == NULL )
238 printk( "error: socket look-up failed\n" );
242 struct can_frame greet;
247 //cegw_can_send( s_can, &greet, *(int*)nla_data(tb[CEGW_CAN_IFINDEX]) );
248 kthread_run(cegw_thread_recv_udp, NULL, "canethgw");
249 kthread_run(cegw_thread_recv_can, NULL, "canethgw");
251 if (tb[CEGW_CMD_INFO] == NULL) {
252 pr_devel("canethgw: CEGW_CMD_INFO is missing in rtmsg\n");
258 switch (*(int*)nla_data(tb[CEGW_CMD_INFO])) {
260 if (!tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT]) {
261 pr_devel("canethgw: missing attribute for CEGW_LISTEN\n");
265 /* ToDo: valid listen address */
266 set = kmalloc(sizeof(*set), GFP_KERNEL);
269 set->eth_ip = *(struct in_addr*)nla_data(tb[CEGW_ETH_IP]);
270 set->eth_port = *(unsigned short*)nla_data(tb[CEGW_ETH_PORT]);
271 /* MS: It would be better to use workqueues here. */
272 kthread_run(cegw_thread_start, set, "canethgw");
274 case CEGW_RULE_ETH_CAN:
275 if (!tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] ||
276 !tb[CEGW_CAN_IFINDEX]) {
277 pr_devel("canethgw: missing attribute for"
278 "CEGW_RULE_ETH_CAN\n");
282 ifindex = *(int*)nla_data(tb[CEGW_CAN_IFINDEX]);
283 ip = *(struct in_addr*)nla_data(tb[CEGW_ETH_IP]);
284 port = *(unsigned short*)nla_data(tb[CEGW_ETH_PORT]);
285 pr_devel("canethgw: new eth->can rule - (%x:%hu)->(%d)\n",
286 ip.s_addr, port, ifindex);
288 rule = kmalloc(sizeof(struct cegw_rule), GFP_KERNEL);
292 rule->can_ifindex = ifindex;
294 rule->eth_port = port;
296 mutex_lock(&rule_eth_can_mutex);
297 hlist_add_head(&rule->list, &rule_eth_can);
298 mutex_unlock(&rule_eth_can_mutex);
300 case CEGW_RULE_CAN_ETH:
301 if (!tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] ||
302 !tb[CEGW_CAN_IFINDEX]) {
303 pr_devel("canethgw: missing attribute for "
304 "CEGW_RULE_CAN_ETH\n");
308 ifindex = *(int*)nla_data(tb[CEGW_CAN_IFINDEX]);
309 ip = *(struct in_addr*)nla_data(tb[CEGW_ETH_IP]);
310 port = *(unsigned short*)nla_data(tb[CEGW_ETH_PORT]);
311 pr_devel("canethgw: new can->eth rule - (%d)->(%x:%hu)\n",
312 ifindex, ip.s_addr, port);
314 rule = kmalloc(sizeof(struct cegw_rule), GFP_KERNEL);
318 rule->can_ifindex = ifindex;
320 rule->eth_port = port;
322 mutex_lock(&rule_can_eth_mutex);
323 hlist_add_head(&rule->list, &rule_can_eth);
324 mutex_unlock(&rule_can_eth_mutex);
327 pr_devel("canethgw: unknown CEGW_CMD_INFO\n");
334 static void cegw_flush(void)
336 struct cegw_rule *rule;
337 struct hlist_node *pos, *n;
339 mutex_lock(&rule_eth_can_mutex);
340 hlist_for_each_entry_safe(rule, pos, n, &rule_eth_can, list) {
341 hlist_del(&rule->list);
344 mutex_unlock(&rule_eth_can_mutex);
346 mutex_lock(&rule_can_eth_mutex);
347 hlist_for_each_entry_safe(rule, pos, n, &rule_can_eth, list) {
348 hlist_del(&rule->list);
351 mutex_unlock(&rule_can_eth_mutex);
354 static int cegw_delroute(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
357 struct nlattr *tb[CEGW_MAX+1];
360 if (nlmsg_len(nlh) < sizeof(*r))
365 if (r->rtm_family != AF_CAN)
366 return -EPFNOSUPPORT;
368 err = nlmsg_parse(nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL);
372 if (tb[CEGW_CMD_INFO] == NULL) {
373 pr_devel("canethgw: CEGW_CMD_INFO is missing in rtmsg\n");
377 if (*(int*)nla_data(tb[CEGW_CMD_INFO]) != CEGW_FLUSH) {
386 static int cegw_put_rule(struct sk_buff *skb, int type, struct cegw_rule *rule)
391 struct nlmsghdr *nlh;
393 ifindex = rule->can_ifindex;
395 port = rule->eth_port;
397 nlh = nlmsg_put(skb, 0, 0, 0, 0, 0);
402 if (nla_put(skb, CEGW_TYPE, sizeof(type), &type) < 0)
405 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(type));
408 if (nla_put(skb, CEGW_CAN_IFINDEX, sizeof(ifindex), &ifindex) < 0)
411 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(ifindex));
414 if (nla_put(skb, CEGW_ETH_IP, sizeof(ip), &ip) < 0)
417 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(ip));
420 if (nla_put(skb, CEGW_ETH_PORT, sizeof(port), &port) < 0)
423 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(port));
428 nlmsg_cancel(skb, nlh);
432 static int cegw_getroute(struct sk_buff *skb, struct netlink_callback *cb)
434 struct cegw_rule *rule;
435 struct hlist_node *pos;
437 int s_idx = cb->args[0];
439 mutex_lock(&rule_eth_can_mutex);
440 mutex_lock(&rule_can_eth_mutex);
441 hlist_for_each_entry(rule, pos, &rule_eth_can, list) {
445 if (cegw_put_rule(skb, CEGW_RULE_ETH_CAN, rule) < 0)
451 hlist_for_each_entry(rule, pos, &rule_can_eth, list) {
455 if (cegw_put_rule(skb, CEGW_RULE_CAN_ETH, rule) < 0)
462 mutex_unlock(&rule_eth_can_mutex);
463 mutex_unlock(&rule_can_eth_mutex);
469 static int cegw_notifier(struct notifier_block *nb, unsigned long msg, void *data)
471 struct net_device *dev = (struct net_device *)data;
472 struct cegw_rule *rule;
473 struct hlist_node *pos, *n;
475 if (!net_eq(dev_net(dev), &init_net))
477 if (dev->type != ARPHRD_CAN)
480 if (msg == NETDEV_UNREGISTER) {
481 hlist_for_each_entry_safe(rule, pos, n, &rule_eth_can, list) {
482 if (rule->can_ifindex == dev->ifindex) {
483 hlist_del(&rule->list);
488 hlist_for_each_entry_safe(rule, pos, n, &rule_can_eth, list) {
489 if (rule->can_ifindex == dev->ifindex) {
490 hlist_del(&rule->list);
499 static int cegw_thread_recv_udp(void *data)
504 struct sockaddr_in sa;
505 struct hlist_node* pos;
509 memset(&mh, 0, sizeof(mh));
511 mh.msg_namelen = sizeof(sa);
512 mh.msg_control = NULL;
513 mh.msg_controllen = 0;
516 printk( "udp receiving\n" );
521 vec.iov_len = sizeof(cf);
523 recv_size = kernel_recvmsg(s_eth, &mh, &vec, 1,
525 printk("udp status=%i\n", recv_size);
531 static int cegw_thread_recv_can(void *data)
536 struct sockaddr_can ca;
537 struct hlist_node* pos;
541 memset(&mh, 0, sizeof(mh));
543 mh.msg_namelen = sizeof(ca);
544 mh.msg_control = NULL;
545 mh.msg_controllen = 0;
548 printk( "can receiving\n" );
553 vec.iov_len = sizeof(cf);
555 recv_size = kernel_recvmsg(s_can, &mh, &vec, 1,
557 printk("can status=%i\n", recv_size);
564 * cegw_thread_stop - stops threads and wait for exit
566 * Waits for threads to stop. Does nothing if cegw_state == CEGW_STOP.
568 static int cegw_thread_stop(void)
571 struct sock *sk = NULL;
573 if (cegw_state == CEGW_STOP)
576 cegw_state = CEGW_STOP;
577 /* shut down socket */
581 sk->sk_shutdown |= how;
582 sk->sk_state_change(sk);
585 kernel_sock_shutdown(udp_sock, SHUT_RDWR);
587 /* wait for return to reuse port if restart */
588 kthread_stop(eth_to_can);
589 kthread_stop(can_to_eth);
590 sock_release(udp_sock);
591 sock_release(can_sock);
598 static int __init cegw_init(void)
600 notifier.notifier_call = cegw_notifier;
601 register_netdevice_notifier(¬ifier);
603 /* subscribe to netlink */
604 rtnl_register(PF_CAN, RTM_GETROUTE, NULL, cegw_getroute, NULL);
605 rtnl_register(PF_CAN, RTM_NEWROUTE, cegw_newroute, NULL, NULL);
606 rtnl_register(PF_CAN, RTM_DELROUTE, cegw_delroute, NULL, NULL);
611 static void __exit cegw_exit(void)
613 /* ToDo: effect on cangw? */
614 rtnl_unregister_all(PF_CAN);
616 /* wait for rtnl callbacks */
620 mutex_lock(&cegw_mutex);
622 cegw_state = CEGW_EXIT;
623 mutex_unlock(&cegw_mutex);
625 unregister_netdevice_notifier(¬ifier);
629 module_init(cegw_init);
630 module_exit(cegw_exit);