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