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                 /* MS: It would be better to use workqueues here. */
 242                 kthread_run(cegw_thread_start, set, "canethgw");
 243                 break;
 244         case CEGW_RULE_ETH_CAN:
 245                 if (!tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] ||
 246                     !tb[CEGW_CAN_IFINDEX]) {
 247                         pr_devel("canethgw: missing attribute for"
 248                                  "CEGW_RULE_ETH_CAN\n");
 249                         return -EINVAL;
 250                 }
 251
 252                 ifindex = *(int*)nla_data(tb[CEGW_CAN_IFINDEX]);
 253                 ip = *(struct in_addr*)nla_data(tb[CEGW_ETH_IP]);
 254                 port = *(unsigned short*)nla_data(tb[CEGW_ETH_PORT]);
 255                 pr_devel("canethgw: new eth->can rule - (%x:%hu)->(%d)\n",
 256                          ip.s_addr, port, ifindex);
 257
 258                 rule = kmalloc(sizeof(struct cegw_rule), GFP_KERNEL);
 259                 if (rule == NULL)
 260                         return -ENOMEM;
 261
 262                 rule->can_ifindex = ifindex;
 263                 rule->eth_ip = ip;
 264                 rule->eth_port = port;
 265
 266                 mutex_lock(&rule_eth_can_mutex);
 267                 hlist_add_head(&rule->list, &rule_eth_can);
 268                 mutex_unlock(&rule_eth_can_mutex);
 269                 break;
 270         case CEGW_RULE_CAN_ETH:
 271                 if (!tb[CEGW_ETH_IP] || !tb[CEGW_ETH_PORT] ||
 272                     !tb[CEGW_CAN_IFINDEX]) {
 273                         pr_devel("canethgw: missing attribute for "
 274                                  "CEGW_RULE_CAN_ETH\n");
 275                         return -EINVAL;
 276                 }
 277
 278                 ifindex = *(int*)nla_data(tb[CEGW_CAN_IFINDEX]);
 279                 ip = *(struct in_addr*)nla_data(tb[CEGW_ETH_IP]);
 280                 port = *(unsigned short*)nla_data(tb[CEGW_ETH_PORT]);
 281                 pr_devel("canethgw: new can->eth rule - (%d)->(%x:%hu)\n",
 282                          ifindex, ip.s_addr, port);
 283
 284                 rule = kmalloc(sizeof(struct cegw_rule), GFP_KERNEL);
 285                 if (rule == NULL)
 286                         return -ENOMEM;
 287
 288                 rule->can_ifindex = ifindex;
 289                 rule->eth_ip = ip;
 290                 rule->eth_port = port;
 291
 292                 mutex_lock(&rule_can_eth_mutex);
 293                 hlist_add_head(&rule->list, &rule_can_eth);
 294                 mutex_unlock(&rule_can_eth_mutex);
 295                 break;
 296         default:
 297                 pr_devel("canethgw: unknown CEGW_CMD_INFO\n");
 298                 break;
 299         }
 300
 301         return 0;
 302 }
 303
 304 static void cegw_flush(void)
 305 {
 306         struct cegw_rule *rule;
 307         struct hlist_node *pos, *n;
 308
 309         mutex_lock(&rule_eth_can_mutex);
 310         hlist_for_each_entry_safe(rule, pos, n, &rule_eth_can, list) {
 311                 hlist_del(&rule->list);
 312                 kfree(rule);
 313         }
 314         mutex_unlock(&rule_eth_can_mutex);
 315
 316         mutex_lock(&rule_can_eth_mutex);
 317         hlist_for_each_entry_safe(rule, pos, n, &rule_can_eth, list) {
 318                 hlist_del(&rule->list);
 319                 kfree(rule);
 320         }
 321         mutex_unlock(&rule_can_eth_mutex);
 322 }
 323
 324 static int cegw_delroute(struct sk_buff *skb, struct nlmsghdr *nlh, void *arg)
 325 {
 326         struct rtmsg *r;
 327         struct nlattr *tb[CEGW_MAX+1];
 328         int err = 0;
 329
 330         if (nlmsg_len(nlh) < sizeof(*r))
 331                 return -EINVAL;
 332
 333         r = nlmsg_data(nlh);
 334
 335         if (r->rtm_family != AF_CAN)
 336                 return -EPFNOSUPPORT;
 337
 338         err = nlmsg_parse(nlh, sizeof(struct rtmsg), tb, CEGW_MAX, NULL);
 339         if (err != 0)
 340                 return -EINVAL;
 341
 342         if (tb[CEGW_CMD_INFO] == NULL) {
 343                 pr_devel("canethgw: CEGW_CMD_INFO is missing in rtmsg\n");
 344                 return -EINVAL;
 345         }
 346
 347         if (*(int*)nla_data(tb[CEGW_CMD_INFO]) != CEGW_FLUSH) {
 348                 return -EINVAL;
 349         }
 350
 351         cegw_flush();
 352
 353         return 0;
 354 }
 355
 356 static int cegw_put_rule(struct sk_buff *skb, int type, struct cegw_rule *rule)
 357 {
 358         int ifindex;
 359         struct in_addr ip;
 360         unsigned short port;
 361         struct nlmsghdr *nlh;
 362
 363         ifindex = rule->can_ifindex;
 364         ip = rule->eth_ip;
 365         port = rule->eth_port;
 366
 367         nlh = nlmsg_put(skb, 0, 0, 0, 0, 0);
 368         if (nlh == NULL)
 369                 return -EMSGSIZE;
 370
 371         /* type */
 372         if (nla_put(skb, CEGW_TYPE, sizeof(type), &type) < 0)
 373                 goto cancel;
 374         else
 375                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(type));
 376
 377         /* can ifindex */
 378         if (nla_put(skb, CEGW_CAN_IFINDEX, sizeof(ifindex), &ifindex) < 0)
 379                 goto cancel;
 380         else
 381                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(ifindex));
 382
 383         /* ip adress */
 384         if (nla_put(skb, CEGW_ETH_IP, sizeof(ip), &ip) < 0)
 385                 goto cancel;
 386         else
 387                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(ip));
 388
 389         /* port */
 390         if (nla_put(skb, CEGW_ETH_PORT, sizeof(port), &port) < 0)
 391                 goto cancel;
 392         else
 393                 nlh->nlmsg_len += NLA_HDRLEN + NLA_ALIGN(sizeof(port));
 394
 395         return skb->len;
 396
 397 cancel:
 398         nlmsg_cancel(skb, nlh);
 399         return -EMSGSIZE;
 400 }
 401
 402 static int cegw_getroute(struct sk_buff *skb, struct netlink_callback *cb)
 403 {
 404         struct cegw_rule *rule;
 405         struct hlist_node *pos;
 406         int idx = 0;
 407         int s_idx = cb->args[0];
 408
 409         mutex_lock(&rule_eth_can_mutex);
 410         mutex_lock(&rule_can_eth_mutex);
 411         hlist_for_each_entry(rule, pos, &rule_eth_can, list) {
 412                 if (idx < s_idx)
 413                         goto cont1;
 414
 415                 if (cegw_put_rule(skb, CEGW_RULE_ETH_CAN, rule) < 0)
 416                         goto brk;
 417 cont1:
 418                 idx++;
 419         }
 420
 421         hlist_for_each_entry(rule, pos, &rule_can_eth, list) {
 422                 if (idx < s_idx)
 423                         goto cont2;
 424
 425                 if (cegw_put_rule(skb, CEGW_RULE_CAN_ETH, rule) < 0)
 426                         goto brk;
 427 cont2:
 428                 idx++;
 429         }
 430
 431 brk:
 432         mutex_unlock(&rule_eth_can_mutex);
 433         mutex_unlock(&rule_can_eth_mutex);
 434         cb->args[0] = idx;
 435
 436         return skb->len;
 437 }
 438
 439 static int cegw_notifier(struct notifier_block *nb, unsigned long msg, void *data)
 440 {
 441         struct net_device *dev = (struct net_device *)data;
 442         struct cegw_rule *rule;
 443         struct hlist_node *pos, *n;
 444
 445         if (!net_eq(dev_net(dev), &init_net))
 446                 return NOTIFY_DONE;
 447         if (dev->type != ARPHRD_CAN)
 448                 return NOTIFY_DONE;
 449
 450         if (msg == NETDEV_UNREGISTER) {
 451                 hlist_for_each_entry_safe(rule, pos, n, &rule_eth_can, list) {
 452                         if (rule->can_ifindex == dev->ifindex) {
 453                                 hlist_del(&rule->list);
 454                                 kfree(rule);
 455                         }
 456                 }
 457
 458                 hlist_for_each_entry_safe(rule, pos, n, &rule_can_eth, list) {
 459                         if (rule->can_ifindex == dev->ifindex) {
 460                                 hlist_del(&rule->list);
 461                                 kfree(rule);
 462                         }
 463                 }
 464         }
 465
 466         return NOTIFY_DONE;
 467 }
 468
 469 /**
 470  * cegw_thread_start - start working threads
 471  * @data: (struct cegw_setting *) with new listening address
 472  *
 473  * Two threads are started. One is serving udp->can routing and the other
 474  * can->udp.
 475  */
 476 static int cegw_thread_start(void *data)
 477 {
 478         struct sockaddr_in udp_addr;
 479         struct sockaddr_can can_addr;
 480         struct cegw_setting *set;
 481
 482         set = (struct cegw_setting *)data;
 483
 484         can_addr.can_family = AF_CAN;
 485         can_addr.can_ifindex = 0;
 486
 487         udp_addr.sin_family = AF_INET;
 488         udp_addr.sin_port = htons(set->eth_port);
 489         udp_addr.sin_addr = set->eth_ip;
 490
 491         kfree(data);
 492         mutex_lock(&cegw_mutex);
 493         if (cegw_state == CEGW_EXIT)
 494                 goto out_err;
 495
 496         /* stops threads if exist */
 497         cegw_thread_stop();
 498
 499         /* create and bind sockets */
 500         if (sock_create_kern(PF_INET, SOCK_DGRAM, IPPROTO_UDP, &udp_sock)
 501             != 0) {
 502                 printk(KERN_ERR "canethgw: udp socket creation failed\n");
 503                 goto out_err;
 504         }
 505
 506         if (sock_create_kern(PF_CAN, SOCK_RAW, CAN_RAW, &can_sock) != 0) {
 507                 printk(KERN_ERR "canethgw: can socket creation failed\n");
 508                 sock_release(udp_sock);
 509                 goto out_err;
 510         }
 511
 512         if (kernel_bind(udp_sock, (struct sockaddr*)&udp_addr,
 513                         sizeof(udp_addr)) != 0) {
 514                 printk(KERN_ERR "canethgw: udp socket binding failed\n");
 515                 sock_release(udp_sock);
 516                 sock_release(can_sock);
 517                 goto out_err;
 518         }
 519
 520         if (kernel_bind(can_sock, (struct sockaddr*) &can_addr,
 521                         sizeof(can_addr)) != 0) {
 522                 printk(KERN_ERR "canethgw: can socket binding failed\n");
 523                 kernel_sock_shutdown(udp_sock, SHUT_RDWR);
 524                 sock_release(udp_sock);
 525                 sock_release(can_sock);
 526                 goto out_err;
 527         }
 528
 529         /* start threads */
 530         cegw_state = CEGW_RUN;
 531
 532         eth_to_can = kthread_create(cegw_udp2can, NULL, "canethgw");
 533         if (IS_ERR(eth_to_can)) {
 534                 cegw_state = CEGW_STOP;
 535                 sock_release(udp_sock);
 536                 sock_release(can_sock);
 537                 goto out_err;
 538         }
 539         get_task_struct(eth_to_can);
 540         wake_up_process(eth_to_can);
 541
 542         can_to_eth = kthread_create(cegw_can2udp, NULL, "canethgw");
 543         if (IS_ERR(can_to_eth)) {
 544                 cegw_state = CEGW_STOP;
 545                 kernel_sock_shutdown(udp_sock, SHUT_RDWR);
 546                 kthread_stop(eth_to_can);
 547                 sock_release(udp_sock);
 548                 sock_release(can_sock);
 549                 goto out_err;
 550         }
 551         get_task_struct(can_to_eth);
 552         wake_up_process(can_to_eth);
 553
 554         mutex_unlock(&cegw_mutex);
 555         pr_devel("threads are running\n");
 556         return 0;
 557 out_err:
 558         mutex_unlock(&cegw_mutex);
 559         return -1;
 560 }
 561
 562 /**
 563  * cegw_thread_stop - stops threads and wait for exit
 564  *
 565  * Waits for threads to stop. Does nothing if cegw_state == CEGW_STOP.
 566  */
 567 static int cegw_thread_stop(void)
 568 {
 569         int how = SHUT_RDWR;
 570         struct sock *sk = NULL;
 571
 572         if (cegw_state == CEGW_STOP)
 573                 return 0;
 574
 575         cegw_state = CEGW_STOP;
 576         /* shut down socket */
 577         sk = can_sock->sk;
 578         how++;
 579         lock_sock(sk);
 580         sk->sk_shutdown |= how;
 581         sk->sk_state_change(sk);
 582         release_sock(sk);
 583
 584         kernel_sock_shutdown(udp_sock, SHUT_RDWR);
 585
 586         /* wait for return to reuse port if restart */
 587         kthread_stop(eth_to_can);
 588         kthread_stop(can_to_eth);
 589         sock_release(udp_sock);
 590         sock_release(can_sock);
 591         can_to_eth = NULL;
 592         eth_to_can = NULL;
 593
 594         return 0;
 595 }
 596
 597 static int __init cegw_init(void)
 598 {
 599         notifier.notifier_call = cegw_notifier;
 600         register_netdevice_notifier(&notifier);
 601
 602         /* subscribe to netlink */
 603         rtnl_register(PF_CAN, RTM_GETROUTE, NULL, cegw_getroute, NULL);
 604         rtnl_register(PF_CAN, RTM_NEWROUTE, cegw_newroute, NULL, NULL);
 605         rtnl_register(PF_CAN, RTM_DELROUTE, cegw_delroute, NULL, NULL);
 606
 607         return 0;
 608 }
 609
 610 static void __exit cegw_exit(void)
 611 {
 612         /* ToDo: effect on cangw? */
 613         rtnl_unregister_all(PF_CAN);
 614
 615         /* wait for rtnl callbacks */
 616         rtnl_lock();
 617         rtnl_unlock();
 618
 619         mutex_lock(&cegw_mutex);
 620         cegw_thread_stop();
 621         cegw_state = CEGW_EXIT;
 622         mutex_unlock(&cegw_mutex);
 623
 624         unregister_netdevice_notifier(&notifier);
 625         cegw_flush();
 626 }
 627
 628 module_init(cegw_init);
 629 module_exit(cegw_exit);
 630