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_udp_can(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_can_udp(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 enum __cegw_state
  35 {
  36         CEGW_RUN,
  37         CEGW_STOP,
  38         CEGW_EXIT
  39 };
  40
  41 struct cegw_rule
  42 {
  43         int can_ifindex;
  44         struct in_addr eth_ip;
  45         unsigned short eth_port;
  46         struct hlist_node list;
  47 };
  48
  49 struct cegw_setting
  50 {
  51         struct in_addr eth_ip;
  52         unsigned short eth_port;
  53 };
  54
  55 static int cegw_state = CEGW_STOP;
  56 static struct socket *can_sock = NULL, *udp_sock = NULL;
  57 static struct task_struct *eth_to_can = NULL, *can_to_eth = NULL;
  58 static struct notifier_block notifier;
  59
  60 static HLIST_HEAD(rule_eth_can);
  61 static HLIST_HEAD(rule_can_eth);
  62 static DEFINE_MUTEX(rule_eth_can_mutex);
  63 static DEFINE_MUTEX(rule_can_eth_mutex);
  64 static DEFINE_MUTEX(cegw_mutex);
  65
  66 static void cegw_udp_send(struct socket *udp_sock, struct can_frame *cf,
  67                 struct in_addr ipaddr, u16 port)
  68 {
  69         struct msghdr mh;
  70         struct sockaddr_in addr;
  71         struct kvec vec;
  72
  73         addr.sin_family = AF_INET;
  74         addr.sin_port = htons(port);
  75         addr.sin_addr = ipaddr;
  76
  77         mh.msg_name = &addr;
  78         mh.msg_namelen = sizeof(addr);
  79         mh.msg_control = NULL;
  80         mh.msg_controllen = 0;
  81         mh.msg_flags = 0;
  82
  83         vec.iov_base = cf;
  84         vec.iov_len = sizeof(*cf);
  85
  86         kernel_sendmsg(udp_sock, &mh, &vec, 1, sizeof(*cf));
  87 }
  88
  89 static void cegw_can_send(struct socket* can_sock, struct can_frame* cf,
  90                 int ifindex)
  91 {
  92         struct msghdr mh;
  93         struct kvec vec;
  94         struct sockaddr_can addr;
  95
  96         addr.can_family = AF_CAN;
  97         addr.can_ifindex = ifindex;
  98
  99         mh.msg_name = &addr;
 100         mh.msg_namelen = sizeof(addr);
 101         mh.msg_control = NULL;
 102         mh.msg_controllen = 0;
 103         mh.msg_flags = 0;
 104
 105         vec.iov_base = cf;
 106         vec.iov_len = sizeof(*cf);
 107
 108         kernel_sendmsg(can_sock, &mh, &vec, 1, sizeof(*cf));
 109 }
 110
 111 /**
 112  * cegw_udp_can - performs udp->can routing
 113  *
 114  * This function is run as a thread.
 115  */
 116 static int cegw_udp_can(void *data)
 117 {
 118         struct can_frame cf;
 119         struct kvec vec;
 120         struct msghdr mh;
 121         struct cegw_rule* rule;
 122         struct hlist_node* pos;
 123         int can_ifidx;
 124         int recv_size;
 125
 126         mh.msg_name = NULL;
 127         mh.msg_namelen = 0;
 128         mh.msg_iov = NULL;
 129         mh.msg_iovlen = 0;
 130         mh.msg_control = NULL;
 131         mh.msg_controllen = 0;
 132         mh.msg_flags = 0;
 133
 134         while (1) {
 135                 if (cegw_state == CEGW_STOP)
 136                         break;
 137                 vec.iov_base = &cf;
 138                 vec.iov_len = sizeof(cf);
 139                 recv_size = kernel_recvmsg(udp_sock, &mh, &vec, 1,
 140                                 sizeof(cf), 0);
 141                 /* recv_size == 0 when shutting down */
 142                 if (recv_size != sizeof(cf) || recv_size == 0)
 143                         continue;
 144                 else if (recv_size < 0)
 145                         return -1;
 146
 147                 mutex_lock(&rule_eth_can_mutex);
 148                 hlist_for_each_entry(rule, pos, &rule_eth_can, list) {
 149                         can_ifidx = rule->can_ifindex;
 150                         /* ToDo: from filter */
 151                         cegw_can_send(can_sock, &cf, can_ifidx);
 152                 }
 153                 mutex_unlock(&rule_eth_can_mutex);
 154         }
 155
 156         return 0;
 157 }
 158
 159 /**
 160  * cegw_can_udp - performs can->udp routing
 161  */
 162 static int cegw_can_udp(void* data)
 163 {
 164         struct msghdr mh;
 165         struct kvec vec;
 166         struct can_frame cf;
 167         struct sockaddr_can ca;
 168         struct cegw_rule* rule;
 169         struct hlist_node* pos;
 170         struct in_addr eth_ip;
 171         u16 eth_port;
 172         int recv_size;
 173
 174         mh.msg_name = &ca;
 175         mh.msg_namelen = sizeof(ca);
 176         mh.msg_control = NULL;
 177         mh.msg_controllen = 0;
 178         mh.msg_flags = 0;
 179
 180         while (1) {
 181                 if (cegw_state == CEGW_STOP)
 182                         break;
 183                 vec.iov_base = &cf;
 184                 vec.iov_len = sizeof(cf);
 185
 186                 recv_size = kernel_recvmsg(can_sock, &mh, &vec, 1,
 187                                            sizeof(cf), 0);
 188                 if (recv_size != sizeof(cf) || recv_size == 0)
 189                         continue;
 190                 else if (recv_size < 0)
 191                         return -1;
 192
 193                 mutex_lock(&rule_can_eth_mutex);
 194                 hlist_for_each_entry(rule, pos, &rule_can_eth, list) {
 195                         eth_ip = rule->eth_ip;
 196                         eth_port = rule->eth_port;
 197                         if (rule->can_ifindex == ca.can_ifindex)
 198                                 cegw_udp_send(udp_sock, &cf, eth_ip, eth_port);
 199                 }
 200                 mutex_unlock(&rule_can_eth_mutex);
 201         }
 202
 203         return 0;
 204 }
 205
 206 static int cegw_newroute(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
 207 {
 208         struct nlattr* tb[ CEGW_MAX+1 ];
 209         struct cegw_rule* rule = NULL;
 210         int ifindex;
 211         struct rtmsg* r;
 212         struct in_addr ip;
 213         unsigned short port;
 214         struct cegw_setting* set;
 215         int err = 0;
 216
 217         if (nlmsg_len(nlh) < sizeof(*r))
 218                 return -EINVAL;
 219
 220         r = nlmsg_data(nlh);
 221
 222         if (r->rtm_family != AF_CAN)
 223                 return -EPFNOSUPPORT;
 224
 225         err = nlmsg_parse(nlh, sizeof(*r), tb, CEGW_MAX, NULL);
 226         if (err < 0) {
 227                 pr_devel("canethgw: nlmsg_parse error\n");
 228                 return err;
 229         }
 230
 231         if (tb[CEGW_CMD_INFO] == NULL) {
 232                 pr_devel("canethgw: CEGW_CMD_INFO is missing in rtmsg\n");
 233                 return -EINVAL;
 234         }
 235
 236         switch (*(int*)nla_data(tb[CEGW_CMD_INFO])) {
 237         case CEGW_LISTEN:
 238                 if (!tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT]) {
 239                         pr_devel("canethgw: missing attribute for CEGW_LISTEN\n");
 240                         return -EINVAL;
 241                 }
 242
 243                 /* ToDo: valid listen address */
 244                 set = kmalloc(sizeof(*set), GFP_KERNEL);
 245                 if (set == NULL)
 246                         return -ENOMEM;
 247                 set->eth_ip   = *(struct in_addr*)nla_data(tb[CEGW_ETH_IP]);
 248                 set->eth_port = *(unsigned short*)nla_data(tb[CEGW_ETH_PORT]);
 249                 kthread_run(cegw_thread_start, set, "canethgw");
 250                 break;
 251         case CEGW_RULE_ETH_CAN:
 252                 if (!tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] ||
 253                     !tb[CEGW_CAN_IFINDEX]) {
 254                         pr_devel("canethgw: missing attribute for"
 255                                  "CEGW_RULE_ETH_CAN\n");
 256                         return -EINVAL;
 257                 }
 258
 259                 ifindex = *(int*)nla_data(tb[CEGW_CAN_IFINDEX]);
 260                 ip = *(struct in_addr*)nla_data(tb[CEGW_ETH_IP]);
 261                 port = *(unsigned short*)nla_data(tb[CEGW_ETH_PORT]);
 262                 pr_devel("canethgw: new eth->can rule - (%x:%hu)->(%d)\n",
 263                          ip.s_addr, port, ifindex);
 264
 265                 rule = kmalloc(sizeof(struct cegw_rule), GFP_KERNEL);
 266                 if (rule == NULL)
 267                         return -ENOMEM;
 268
 269                 rule->can_ifindex = ifindex;
 270                 rule->eth_ip = ip;
 271                 rule->eth_port = port;
 272
 273                 mutex_lock(&rule_eth_can_mutex);
 274                 hlist_add_head(&rule->list, &rule_eth_can);
 275                 mutex_unlock(&rule_eth_can_mutex);
 276                 break;
 277         case CEGW_RULE_CAN_ETH:
 278                 if (!tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] ||
 279                     !tb[CEGW_CAN_IFINDEX]) {
 280                         pr_devel("canethgw: missing attribute for "
 281                                  "CEGW_RULE_CAN_ETH\n");
 282                         return -EINVAL;
 283                 }
 284
 285                 ifindex = *(int*)nla_data(tb[CEGW_CAN_IFINDEX]);
 286                 ip = *(struct in_addr*)nla_data(tb[CEGW_ETH_IP]);
 287                 port = *(unsigned short*)nla_data(tb[CEGW_ETH_PORT]);
 288                 pr_devel("canethgw: new can->eth rule - (%d)->(%x:%hu)\n",
 289                          ifindex, ip.s_addr, port);
 290
 291                 rule = kmalloc(sizeof(struct cegw_rule), GFP_KERNEL);
 292                 if (rule == NULL)
 293                         return -ENOMEM;
 294
 295                 rule->can_ifindex = ifindex;
 296                 rule->eth_ip = ip;
 297                 rule->eth_port = port;
 298
 299                 mutex_lock(&rule_can_eth_mutex);
 300                 hlist_add_head(&rule->list, &rule_can_eth);
 301                 mutex_unlock(&rule_can_eth_mutex);
 302                 break;
 303         default:
 304                 pr_devel("canethgw: unknown CEGW_CMD_INFO\n");
 305                 break;
 306         }
 307
 308         return 0;
 309 }
 310
 311 static void cegw_flush(void)
 312 {
 313         struct cegw_rule *rule;
 314         struct hlist_node *pos, *n;
 315
 316         mutex_lock(&rule_eth_can_mutex);
 317         hlist_for_each_entry_safe(rule, pos, n, &rule_eth_can, list) {
 318                 hlist_del(&rule->list);
 319                 kfree(rule);
 320         }
 321         mutex_unlock(&rule_eth_can_mutex);
 322
 323         mutex_lock(&rule_can_eth_mutex);
 324         hlist_for_each_entry_safe(rule, pos, n, &rule_can_eth, list) {
 325                 hlist_del(&rule->list);
 326                 kfree(rule);
 327         }
 328         mutex_unlock(&rule_can_eth_mutex);
 329 }
 330
 331 static int cegw_delroute(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
 332 {
 333         struct rtmsg *r;
 334         struct nlattr *tb[CEGW_MAX+1];
 335         int err = 0;
 336
 337         if (nlmsg_len(nlh) < sizeof(*r))
 338                 return -EINVAL;
 339
 340         r = nlmsg_data(nlh);
 341
 342         if (r->rtm_family != AF_CAN)
 343                 return -EPFNOSUPPORT;
 344
 345         err = nlmsg_parse(nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL);
 346         if (err != 0)
 347                 return -EINVAL;
 348
 349         if (tb[CEGW_CMD_INFO] == NULL) {
 350                 pr_devel("canethgw: CEGW_CMD_INFO is missing in rtmsg\n");
 351                 return -EINVAL;
 352         }
 353
 354         if (*(int*)nla_data(tb[CEGW_CMD_INFO]) != CEGW_FLUSH) {
 355                 return -EINVAL;
 356         }
 357
 358         cegw_flush();
 359
 360         return 0;
 361 }
 362
 363 static int cegw_put_rule(struct sk_buff *skb, int type, struct cegw_rule *rule)
 364 {
 365         int ifindex;
 366         struct in_addr ip;
 367         unsigned short port;
 368         struct nlmsghdr *nlh;
 369
 370         ifindex = rule->can_ifindex;
 371         ip = rule->eth_ip;
 372         port = rule->eth_port;
 373
 374         nlh = nlmsg_put(skb, 0, 0, 0, 0, 0);
 375         if (nlh == NULL)
 376                 return -EMSGSIZE;
 377
 378         /* type */
 379         if (nla_put(skb, CEGW_TYPE, sizeof(type), &type) < 0)
 380                 goto cancel;
 381         else
 382                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(type));
 383
 384         /* can ifindex */
 385         if (nla_put(skb, CEGW_CAN_IFINDEX, sizeof(ifindex), &ifindex) < 0)
 386                 goto cancel;
 387         else
 388                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(ifindex));
 389
 390         /* ip adress */
 391         if (nla_put(skb, CEGW_ETH_IP, sizeof(ip), &ip) < 0)
 392                 goto cancel;
 393         else
 394                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(ip));
 395
 396         /* port */
 397         if (nla_put(skb, CEGW_ETH_PORT, sizeof(port), &port) < 0)
 398                 goto cancel;
 399         else
 400                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(port));
 401
 402         return skb->len;
 403
 404 cancel:
 405         nlmsg_cancel(skb, nlh);
 406         return -EMSGSIZE;
 407 }
 408
 409 static int cegw_getroute(struct sk_buff *skb, struct netlink_callback *cb)
 410 {
 411         struct cegw_rule *rule;
 412         struct hlist_node *pos;
 413         int idx = 0;
 414         int s_idx = cb->args[0];
 415
 416         mutex_lock(&rule_eth_can_mutex);
 417         mutex_lock(&rule_can_eth_mutex);
 418         hlist_for_each_entry(rule, pos, &rule_eth_can, list) {
 419                 if (idx < s_idx)
 420                         goto cont1;
 421
 422                 if (cegw_put_rule(skb, CEGW_RULE_ETH_CAN, rule) < 0)
 423                         goto brk;
 424 cont1:
 425                 idx++;
 426         }
 427
 428         hlist_for_each_entry(rule, pos, &rule_can_eth, list) {
 429                 if (idx < s_idx)
 430                         goto cont2;
 431
 432                 if (cegw_put_rule(skb, CEGW_RULE_CAN_ETH, rule) < 0)
 433                         goto brk;
 434 cont2:
 435                 idx++;
 436         }
 437
 438 brk:
 439         mutex_unlock(&rule_eth_can_mutex);
 440         mutex_unlock(&rule_can_eth_mutex);
 441         cb->args[0] = idx;
 442
 443         return skb->len;
 444 }
 445
 446 static int cegw_notifier(struct notifier_block *nb, unsigned long msg, void *data)
 447 {
 448         struct net_device *dev = (struct net_device *)data;
 449         struct cegw_rule *rule;
 450         struct hlist_node *pos, *n;
 451
 452         if (!net_eq(dev_net(dev), &init_net))
 453                 return NOTIFY_DONE;
 454         if (dev->type != ARPHRD_CAN)
 455                 return NOTIFY_DONE;
 456
 457         if (msg == NETDEV_UNREGISTER) {
 458                 hlist_for_each_entry_safe(rule, pos, n, &rule_eth_can, list) {
 459                         if (rule->can_ifindex == dev->ifindex) {
 460                                 hlist_del(&rule->list);
 461                                 kfree(rule);
 462                         }
 463                 }
 464
 465                 hlist_for_each_entry_safe(rule, pos, n, &rule_can_eth, list) {
 466                         if (rule->can_ifindex == dev->ifindex) {
 467                                 hlist_del(&rule->list);
 468                                 kfree(rule);
 469                         }
 470                 }
 471         }
 472
 473         return NOTIFY_DONE;
 474 }
 475
 476 /**
 477  * cegw_thread_start - start working threads
 478  * @data: (struct cegw_setting *) with new listening address
 479  *
 480  * Two threads are started. One is serving udp->can routing and the other
 481  * can->udp.
 482  */
 483 static int cegw_thread_start(void *data)
 484 {
 485         struct sockaddr_in udp_addr;
 486         struct sockaddr_can can_addr;
 487         struct cegw_setting *set;
 488
 489         set = (struct cegw_setting *)data;
 490
 491         can_addr.can_family = AF_CAN;
 492         can_addr.can_ifindex = 0;
 493
 494         udp_addr.sin_family = AF_INET;
 495         udp_addr.sin_port = htons(set->eth_port);
 496         udp_addr.sin_addr = set->eth_ip;
 497
 498         kfree(data);
 499         mutex_lock(&cegw_mutex);
 500         if (cegw_state == CEGW_EXIT)
 501                 goto out_err;
 502
 503         /* stops threads if exist */
 504         cegw_thread_stop();
 505
 506         /* create and bind sockets */
 507         if (sock_create_kern(PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock)
 508             != 0) {
 509                 printk(KERN_ERR "canethgw: udp socket creation failed\n");
 510                 goto out_err;
 511         }
 512
 513         if (sock_create_kern(PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0) {
 514                 printk(KERN_ERR "canethgw: can socket creation failed\n");
 515                 sock_release(udp_sock);
 516                 goto out_err;
 517         }
 518
 519         if (kernel_bind(udp_sock, (struct sockaddr*)&udp_addr,
 520                         sizeof(udp_addr)) != 0) {
 521                 printk(KERN_ERR "canethgw: udp socket binding failed\n");
 522                 sock_release(udp_sock);
 523                 sock_release(can_sock);
 524                 goto out_err;
 525         }
 526
 527         if (kernel_bind(can_sock, (struct sockaddr*) &can_addr,
 528                         sizeof(can_addr)) != 0) {
 529                 printk(KERN_ERR "canethgw: can socket binding failed\n");
 530                 kernel_sock_shutdown(udp_sock, SHUT_RDWR);
 531                 sock_release(udp_sock);
 532                 sock_release(can_sock);
 533                 goto out_err;
 534         }
 535
 536         /* start threads */
 537         cegw_state = CEGW_RUN;
 538
 539         eth_to_can = kthread_create(cegw_udp_can, NULL, "canethgw");
 540         if (IS_ERR(eth_to_can)) {
 541                 cegw_state = CEGW_STOP;
 542                 sock_release(udp_sock);
 543                 sock_release(can_sock);
 544                 goto out_err;
 545         }
 546         get_task_struct(eth_to_can);
 547         wake_up_process(eth_to_can);
 548
 549         can_to_eth = kthread_create(cegw_can_udp, NULL, "canethgw");
 550         if (IS_ERR(can_to_eth)) {
 551                 cegw_state = CEGW_STOP;
 552                 kernel_sock_shutdown(udp_sock, SHUT_RDWR);
 553                 kthread_stop(eth_to_can);
 554                 sock_release(udp_sock);
 555                 sock_release(can_sock);
 556                 goto out_err;
 557         }
 558         get_task_struct(can_to_eth);
 559         wake_up_process(can_to_eth);
 560
 561         mutex_unlock(&cegw_mutex);
 562         pr_devel("threads are running\n");
 563         return 0;
 564 out_err:
 565         mutex_unlock(&cegw_mutex);
 566         return -1;
 567 }
 568
 569 /**
 570  * cegw_thread_stop - stops threads and wait for exit
 571  *
 572  * Waits for threads to stop. Does nothing if cegw_state == CEGW_STOP.
 573  */
 574 static int cegw_thread_stop(void)
 575 {
 576         int how = SHUT_RDWR;
 577         struct sock *sk = NULL;
 578
 579         if (cegw_state == CEGW_STOP)
 580                 return 0;
 581
 582         cegw_state = CEGW_STOP;
 583         /* shut down socket */
 584         sk = can_sock->sk;
 585         how++;
 586         lock_sock(sk);
 587         sk->sk_shutdown |= how;
 588         sk->sk_state_change(sk);
 589         release_sock(sk);
 590
 591         kernel_sock_shutdown(udp_sock, SHUT_RDWR);
 592
 593         /* wait for return to reuse port if restart */
 594         kthread_stop(eth_to_can);
 595         kthread_stop(can_to_eth);
 596         sock_release(udp_sock);
 597         sock_release(can_sock);
 598         can_to_eth = NULL;
 599         eth_to_can = NULL;
 600
 601         return 0;
 602 }
 603
 604 static int __init cegw_init(void)
 605 {
 606         notifier.notifier_call = cegw_notifier;
 607         register_netdevice_notifier(&notifier);
 608
 609         /* subscribe to netlink */
 610         rtnl_register(PF_CAN, RTM_GETROUTE, NULL, cegw_getroute, NULL);
 611         rtnl_register(PF_CAN, RTM_NEWROUTE, cegw_newroute, NULL, NULL);
 612         rtnl_register(PF_CAN, RTM_DELROUTE, cegw_delroute, NULL, NULL);
 613
 614         return 0;
 615 }
 616
 617 static void __exit cegw_exit(void)
 618 {
 619         /* ToDo: effect on cangw? */
 620         rtnl_unregister_all(PF_CAN);
 621
 622         /* wait for rtnl callbacks */
 623         rtnl_lock();
 624         rtnl_unlock();
 625
 626         mutex_lock(&cegw_mutex);
 627         cegw_thread_stop();
 628         cegw_state = CEGW_EXIT;
 629         mutex_unlock(&cegw_mutex);
 630
 631         unregister_netdevice_notifier(&notifier);
 632         cegw_flush();
 633 }
 634
 635 module_init(cegw_init);
 636 module_exit(cegw_exit);
 637