kernel/canethgw.c

   1 #define DEBUG
   2
   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>
  17 #include <net/sock.h>
  18 #include "canethgw.h"
  19 #include <linux/completion.h>
  20 #include <linux/mutex.h>
  21 #include <net/inet_common.h>
  22
  23 MODULE_LICENSE("GPL");
  24
  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,
  30                           int ifindex);
  31 static int cegw_thread_start(void *data);
  32 static int cegw_thread_stop(void);
  33
  34 static int cegw_thread_recv_udp(void *data);
  35 static int cegw_thread_recv_can(void *data);
  36
  37 enum __cegw_state {
  38         CEGW_RUN,
  39         CEGW_STOP,
  40         CEGW_EXIT
  41 };
  42
  43 struct cegw_rule {
  44         int can_ifindex;
  45         struct in_addr eth_ip;
  46         unsigned short eth_port;
  47         struct hlist_node list;
  48 };
  49
  50 struct cegw_setting {
  51         struct in_addr eth_ip;
  52         unsigned short eth_port;
  53 };
  54
  55
  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;
  61
  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);
  67
  68 static void cegw_udp_send(struct socket *udp_sock, struct can_frame *cf,
  69                           struct in_addr ipaddr, u16 port)
  70 {
  71         struct msghdr mh;
  72         struct sockaddr_in addr;
  73         struct kvec vec;
  74
  75         addr.sin_family = AF_INET;
  76         addr.sin_port = htons(port);
  77         addr.sin_addr = ipaddr;
  78
  79         mh.msg_name = &addr;
  80         mh.msg_namelen = sizeof(addr);
  81         mh.msg_control = NULL;
  82         mh.msg_controllen = 0;
  83         mh.msg_flags = 0;
  84
  85         vec.iov_base = cf;
  86         vec.iov_len = sizeof(*cf);
  87
  88         /* FIXME: Convert endianing of cf->can_id */
  89         kernel_sendmsg(udp_sock, &mh, &vec, 1, sizeof(*cf));
  90 }
  91
  92 static void cegw_can_send(struct socket* can_sock, struct can_frame* cf,
  93                 int ifindex)
  94 {
  95         struct msghdr mh;
  96         struct kvec vec;
  97         struct sockaddr_can addr;
  98
  99         addr.can_family = AF_CAN;
 100         addr.can_ifindex = ifindex;
 101
 102         mh.msg_name = &addr;
 103         mh.msg_namelen = sizeof(addr);
 104         mh.msg_control = NULL;
 105         mh.msg_controllen = 0;
 106         mh.msg_flags = 0;
 107
 108         vec.iov_base = cf;
 109         vec.iov_len = sizeof(*cf);
 110
 111         kernel_sendmsg(can_sock, &mh, &vec, 1, sizeof(*cf));
 112 }
 113
 114 /**
 115  * cegw_udp2can - performs udp->can routing
 116  *
 117  * This function is run as a thread.
 118  */
 119 static int cegw_udp2can(void *data)
 120 {
 121         struct can_frame cf;
 122         struct kvec vec;
 123         struct msghdr mh;
 124         struct cegw_rule* rule;
 125         struct hlist_node* pos;
 126         int can_ifidx;
 127         int recv_size;
 128
 129         memset(&mh, 0, sizeof(mh));
 130
 131         while (cegw_state != CEGW_STOP) {
 132                 vec.iov_base = &cf;
 133                 vec.iov_len = sizeof(cf);
 134                 recv_size = kernel_recvmsg(udp_sock, &mh, &vec, 1,
 135                                 sizeof(cf), 0);
 136                 /* recv_size == 0 when shutting down */
 137                 if (recv_size != sizeof(cf) || recv_size == 0)
 138                         continue;
 139                 else if (recv_size < 0)
 140                         return -1;
 141
 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);
 148                 }
 149                 mutex_unlock(&rule_eth_can_mutex);
 150         }
 151
 152         return 0;
 153 }
 154
 155 /**
 156  * cegw_can2udp - performs can->udp routing
 157  *
 158  * Runs as a thread.
 159  */
 160 static int cegw_can2udp(void* data)
 161 {
 162         struct msghdr mh;
 163         struct kvec vec;
 164         struct can_frame cf;
 165         struct sockaddr_can ca;
 166         struct cegw_rule* rule;
 167         struct hlist_node* pos;
 168         struct in_addr eth_ip;
 169         u16 eth_port;
 170         int recv_size;
 171
 172         mh.msg_name = &ca;
 173         mh.msg_namelen = sizeof(ca);
 174         mh.msg_control = NULL;
 175         mh.msg_controllen = 0;
 176         mh.msg_flags = 0;
 177
 178         while (cegw_state != CEGW_STOP) {
 179                 vec.iov_base = &cf;
 180                 vec.iov_len = sizeof(cf);
 181
 182                 recv_size = kernel_recvmsg(can_sock, &mh, &vec, 1,
 183                                            sizeof(cf), 0);
 184                 if (recv_size != sizeof(cf) || recv_size == 0)
 185                         continue;
 186                 else if (recv_size < 0)
 187                         return -1;
 188
 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);
 195                 }
 196                 mutex_unlock(&rule_can_eth_mutex);
 197         }
 198
 199         return 0;
 200 }
 201
 202 static int cegw_newroute(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
 203 {
 204         struct nlattr* tb[ CEGW_MAX+1 ];
 205         struct cegw_rule* rule = NULL;
 206         int ifindex;
 207         struct rtmsg* r;
 208         struct in_addr ip;
 209         unsigned short port;
 210         struct cegw_setting* set;
 211         int err = 0;
 212         int fd_eth, fd_can;
 213
 214         if (nlmsg_len(nlh) < sizeof(*r))
 215                 return -EINVAL;
 216
 217         r = nlmsg_data(nlh);
 218
 219         if (r->rtm_family != AF_CAN)
 220                 return -EPFNOSUPPORT;
 221
 222         err = nlmsg_parse(nlh, sizeof(*r), tb, CEGW_MAX, NULL);
 223         if (err < 0) {
 224                 pr_devel("canethgw: nlmsg_parse error\n");
 225                 return err;
 226         }
 227
 228         fd_eth = *(int*)nla_data(tb[CEGW_ETH_SOCK]);
 229         fd_can = *(int*)nla_data(tb[CEGW_CAN_SOCK]);
 230
 231         printk( "cegw: sock=%i,%i\n", fd_eth, fd_can );
 232
 233         s_can = sockfd_lookup( fd_can, &err );
 234         s_eth = sockfd_lookup( fd_eth, &err );
 235
 236         if( s_can == NULL || s_eth == NULL )
 237         {
 238                 printk( "error: socket look-up failed\n" );
 239                 return 0;
 240         }
 241
 242         struct can_frame greet;
 243         greet.can_id = 0;
 244         greet.can_dlc = 4;
 245         greet.data[0] = 7;
 246
 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");
 250         /*
 251         if (tb[CEGW_CMD_INFO] == NULL) {
 252                 pr_devel("canethgw: CEGW_CMD_INFO is missing in rtmsg\n");
 253                 return -EINVAL;
 254         }
 255         */
 256
 257         return 0;
 258         switch (*(int*)nla_data(tb[CEGW_CMD_INFO])) {
 259         case CEGW_LISTEN:
 260                 if (!tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT]) {
 261                         pr_devel("canethgw: missing attribute for CEGW_LISTEN\n");
 262                         return -EINVAL;
 263                 }
 264
 265                 /* ToDo: valid listen address */
 266                 set = kmalloc(sizeof(*set), GFP_KERNEL);
 267                 if (set == NULL)
 268                         return -ENOMEM;
 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");
 273                 break;
 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");
 279                         return -EINVAL;
 280                 }
 281
 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);
 287
 288                 rule = kmalloc(sizeof(struct cegw_rule), GFP_KERNEL);
 289                 if (rule == NULL)
 290                         return -ENOMEM;
 291
 292                 rule->can_ifindex = ifindex;
 293                 rule->eth_ip = ip;
 294                 rule->eth_port = port;
 295
 296                 mutex_lock(&rule_eth_can_mutex);
 297                 hlist_add_head(&rule->list, &rule_eth_can);
 298                 mutex_unlock(&rule_eth_can_mutex);
 299                 break;
 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");
 305                         return -EINVAL;
 306                 }
 307
 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);
 313
 314                 rule = kmalloc(sizeof(struct cegw_rule), GFP_KERNEL);
 315                 if (rule == NULL)
 316                         return -ENOMEM;
 317
 318                 rule->can_ifindex = ifindex;
 319                 rule->eth_ip = ip;
 320                 rule->eth_port = port;
 321
 322                 mutex_lock(&rule_can_eth_mutex);
 323                 hlist_add_head(&rule->list, &rule_can_eth);
 324                 mutex_unlock(&rule_can_eth_mutex);
 325                 break;
 326         default:
 327                 pr_devel("canethgw: unknown CEGW_CMD_INFO\n");
 328                 break;
 329         }
 330
 331         return 0;
 332 }
 333
 334 static void cegw_flush(void)
 335 {
 336         struct cegw_rule *rule;
 337         struct hlist_node *pos, *n;
 338
 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);
 342                 kfree(rule);
 343         }
 344         mutex_unlock(&rule_eth_can_mutex);
 345
 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);
 349                 kfree(rule);
 350         }
 351         mutex_unlock(&rule_can_eth_mutex);
 352 }
 353
 354 static int cegw_delroute(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
 355 {
 356         struct rtmsg *r;
 357         struct nlattr *tb[CEGW_MAX+1];
 358         int err = 0;
 359
 360         if (nlmsg_len(nlh) < sizeof(*r))
 361                 return -EINVAL;
 362
 363         r = nlmsg_data(nlh);
 364
 365         if (r->rtm_family != AF_CAN)
 366                 return -EPFNOSUPPORT;
 367
 368         err = nlmsg_parse(nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL);
 369         if (err != 0)
 370                 return -EINVAL;
 371
 372         if (tb[CEGW_CMD_INFO] == NULL) {
 373                 pr_devel("canethgw: CEGW_CMD_INFO is missing in rtmsg\n");
 374                 return -EINVAL;
 375         }
 376
 377         if (*(int*)nla_data(tb[CEGW_CMD_INFO]) != CEGW_FLUSH) {
 378                 return -EINVAL;
 379         }
 380
 381         cegw_flush();
 382
 383         return 0;
 384 }
 385
 386 static int cegw_put_rule(struct sk_buff *skb, int type, struct cegw_rule *rule)
 387 {
 388         int ifindex;
 389         struct in_addr ip;
 390         unsigned short port;
 391         struct nlmsghdr *nlh;
 392
 393         ifindex = rule->can_ifindex;
 394         ip = rule->eth_ip;
 395         port = rule->eth_port;
 396
 397         nlh = nlmsg_put(skb, 0, 0, 0, 0, 0);
 398         if (nlh == NULL)
 399                 return -EMSGSIZE;
 400
 401         /* type */
 402         if (nla_put(skb, CEGW_TYPE, sizeof(type), &type) < 0)
 403                 goto cancel;
 404         else
 405                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(type));
 406
 407         /* can ifindex */
 408         if (nla_put(skb, CEGW_CAN_IFINDEX, sizeof(ifindex), &ifindex) < 0)
 409                 goto cancel;
 410         else
 411                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(ifindex));
 412
 413         /* ip adress */
 414         if (nla_put(skb, CEGW_ETH_IP, sizeof(ip), &ip) < 0)
 415                 goto cancel;
 416         else
 417                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(ip));
 418
 419         /* port */
 420         if (nla_put(skb, CEGW_ETH_PORT, sizeof(port), &port) < 0)
 421                 goto cancel;
 422         else
 423                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(port));
 424
 425         return skb->len;
 426
 427 cancel:
 428         nlmsg_cancel(skb, nlh);
 429         return -EMSGSIZE;
 430 }
 431
 432 static int cegw_getroute(struct sk_buff *skb, struct netlink_callback *cb)
 433 {
 434         struct cegw_rule *rule;
 435         struct hlist_node *pos;
 436         int idx = 0;
 437         int s_idx = cb->args[0];
 438
 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) {
 442                 if (idx < s_idx)
 443                         goto cont1;
 444
 445                 if (cegw_put_rule(skb, CEGW_RULE_ETH_CAN, rule) < 0)
 446                         goto brk;
 447 cont1:
 448                 idx++;
 449         }
 450
 451         hlist_for_each_entry(rule, pos, &rule_can_eth, list) {
 452                 if (idx < s_idx)
 453                         goto cont2;
 454
 455                 if (cegw_put_rule(skb, CEGW_RULE_CAN_ETH, rule) < 0)
 456                         goto brk;
 457 cont2:
 458                 idx++;
 459         }
 460
 461 brk:
 462         mutex_unlock(&rule_eth_can_mutex);
 463         mutex_unlock(&rule_can_eth_mutex);
 464         cb->args[0] = idx;
 465
 466         return skb->len;
 467 }
 468
 469 static int cegw_notifier(struct notifier_block *nb, unsigned long msg, void *data)
 470 {
 471         struct net_device *dev = (struct net_device *)data;
 472         struct cegw_rule *rule;
 473         struct hlist_node *pos, *n;
 474
 475         if (!net_eq(dev_net(dev), &init_net))
 476                 return NOTIFY_DONE;
 477         if (dev->type != ARPHRD_CAN)
 478                 return NOTIFY_DONE;
 479
 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);
 484                                 kfree(rule);
 485                         }
 486                 }
 487
 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);
 491                                 kfree(rule);
 492                         }
 493                 }
 494         }
 495
 496         return NOTIFY_DONE;
 497 }
 498
 499 static int cegw_thread_recv_udp(void *data)
 500 {
 501         struct can_frame cf;
 502         struct kvec vec;
 503         struct msghdr mh;
 504         struct sockaddr_in sa;
 505         struct hlist_node* pos;
 506         int can_ifidx;
 507         int recv_size;
 508
 509         memset(&mh, 0, sizeof(mh));
 510         mh.msg_name = &sa;
 511         mh.msg_namelen = sizeof(sa);
 512         mh.msg_control = NULL;
 513         mh.msg_controllen = 0;
 514         mh.msg_flags = 0;
 515
 516         printk( "udp receiving\n" );
 517
 518         while( 1 )
 519         {
 520                 vec.iov_base = &cf;
 521                 vec.iov_len = sizeof(cf);
 522
 523                 recv_size = kernel_recvmsg(s_eth, &mh, &vec, 1,
 524                                 sizeof(cf), 0);
 525                 printk("udp status=%i\n", recv_size);
 526         }
 527
 528         return 0;
 529 }
 530
 531 static int cegw_thread_recv_can(void *data)
 532 {
 533         struct can_frame cf;
 534         struct kvec vec;
 535         struct msghdr mh;
 536         struct sockaddr_can ca;
 537         struct hlist_node* pos;
 538         int can_ifidx;
 539         int recv_size;
 540
 541         memset(&mh, 0, sizeof(mh));
 542         mh.msg_name = &ca;
 543         mh.msg_namelen = sizeof(ca);
 544         mh.msg_control = NULL;
 545         mh.msg_controllen = 0;
 546         mh.msg_flags = 0;
 547
 548         printk( "can receiving\n" );
 549
 550         while( 1 )
 551         {
 552                 vec.iov_base = &cf;
 553                 vec.iov_len = sizeof(cf);
 554
 555                 recv_size = kernel_recvmsg(s_can, &mh, &vec, 1,
 556                                 sizeof(cf), 0);
 557                 printk("can status=%i\n", recv_size);
 558         }
 559
 560         return 0;
 561 }
 562
 563 /**
 564  * cegw_thread_stop - stops threads and wait for exit
 565  *
 566  * Waits for threads to stop. Does nothing if cegw_state == CEGW_STOP.
 567  */
 568 static int cegw_thread_stop(void)
 569 {
 570         int how = SHUT_RDWR;
 571         struct sock *sk = NULL;
 572
 573         if (cegw_state == CEGW_STOP)
 574                 return 0;
 575
 576         cegw_state = CEGW_STOP;
 577         /* shut down socket */
 578         sk = can_sock->sk;
 579         how++;
 580         lock_sock(sk);
 581         sk->sk_shutdown |= how;
 582         sk->sk_state_change(sk);
 583         release_sock(sk);
 584
 585         kernel_sock_shutdown(udp_sock, SHUT_RDWR);
 586
 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);
 592         can_to_eth = NULL;
 593         eth_to_can = NULL;
 594
 595         return 0;
 596 }
 597
 598 static int __init cegw_init(void)
 599 {
 600         notifier.notifier_call = cegw_notifier;
 601         register_netdevice_notifier(&notifier);
 602
 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);
 607
 608         return 0;
 609 }
 610
 611 static void __exit cegw_exit(void)
 612 {
 613         /* ToDo: effect on cangw? */
 614         rtnl_unregister_all(PF_CAN);
 615
 616         /* wait for rtnl callbacks */
 617         rtnl_lock();
 618         rtnl_unlock();
 619
 620         mutex_lock(&cegw_mutex);
 621         cegw_thread_stop();
 622         cegw_state = CEGW_EXIT;
 623         mutex_unlock(&cegw_mutex);
 624
 625         unregister_netdevice_notifier(&notifier);
 626         cegw_flush();
 627 }
 628
 629 module_init(cegw_init);
 630 module_exit(cegw_exit);
 631