src/kernel/mptp.c

   1 #include <linux/module.h>
   2 #include <linux/version.h>
   3 #include <net/sock.h>
   4 #include <net/protocol.h>
   5 #include <net/ip.h>
   6 #include <net/route.h>
   7
   8 #include "mptp.h"
   9 #include "debug.h"
  10
  11 MODULE_DESCRIPTION("Multi-Party Transport Protocol");
  12 MODULE_AUTHOR("Adrian Bondrescu/Cornel Mercan");
  13 MODULE_LICENSE("GPL");
  14
  15 struct mptp_sock {
  16         struct inet_sock sock;
  17         /* mptp socket speciffic data */
  18         uint16_t src;
  19         uint16_t dst;
  20 };
  21
  22 static struct mptp_sock * sock_port_map[MAX_MPTP_PORT];
  23
  24 static inline struct mptp_sock * mptp_sk(struct sock * sock)
  25 {
  26         return (struct mptp_sock *)(sock);
  27 }
  28
  29 static inline struct mptphdr * mptp_hdr(const struct sk_buff * skb)
  30 {
  31         return (struct mptphdr *) skb_transport_header(skb);
  32 }
  33
  34 static inline uint16_t get_next_free_port(void)
  35 {
  36         int i;
  37         for (i = MIN_MPTP_PORT; i < MAX_MPTP_PORT; i ++)
  38                 if (sock_port_map[i] == NULL)
  39                         return i;
  40         return 0;
  41 }
  42
  43 static inline void mptp_unhash(uint16_t port)
  44 {
  45         sock_port_map[port] = NULL;
  46 }
  47
  48 static inline void mptp_hash(uint16_t port, struct mptp_sock *ssh)
  49 {
  50         sock_port_map[port] = ssh;
  51 }
  52
  53 static inline struct mptp_sock * mptp_lookup(uint16_t port)
  54 {
  55         return sock_port_map[port];
  56 }
  57
  58 static int mptp_release(struct socket *sock)
  59 {
  60         struct sock *sk = sock->sk;
  61         struct mptp_sock * ssk = mptp_sk(sk);
  62
  63         if (unlikely(!sk))
  64                 return 0;
  65
  66         mptp_unhash(ssk->src);
  67
  68         sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
  69
  70         synchronize_net();
  71
  72         sock_orphan(sk);
  73         sock->sk = NULL;
  74
  75         skb_queue_purge(&sk->sk_receive_queue);
  76
  77         log_debug("mptp_release sock=%p\n", sk);
  78         sock_put(sk);
  79
  80         return 0;
  81 }
  82
  83 static int mptp_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
  84 {
  85         struct sockaddr_mptp *mptp_addr;
  86         struct mptp_sock *ssk;
  87         int err;
  88         uint16_t port;
  89
  90         if (unlikely(addr_len < sizeof(struct sockaddr_mptp) + sizeof(struct mptp_dest))) {
  91                 log_error("Invalid size for sockaddr (%d)\n", addr_len);
  92                 err = -EINVAL;
  93                 goto out;
  94         }
  95
  96         mptp_addr = (struct sockaddr_mptp *) addr;
  97
  98         log_debug("Bind received port=%u (network order)\n", mptp_addr->dests[0].port);
  99         port = ntohs(mptp_addr->dests[0].port);
 100         if (port == 0)
 101                 port = get_next_free_port();
 102
 103         if (unlikely(port == 0 || port >= MAX_MPTP_PORT)) {
 104                 log_error("Invalid value for sockaddr port (%u)\n", port);
 105                 err = -EINVAL;
 106                 goto out;
 107         }
 108
 109         if (unlikely(mptp_lookup(port) != NULL)) {
 110                 log_error("Port %u already in use\n", port);
 111                 err = -EADDRINUSE;
 112                 goto out;
 113         }
 114
 115         ssk = mptp_sk(sock->sk);
 116     sock->sk->sk_rcvbuf = 10 * 1024 * 1024;
 117         ssk->src = port;
 118
 119         mptp_hash(port, ssk);
 120
 121         log_debug("Socket %p bound to port %u\n", ssk, port);
 122
 123         return 0;
 124
 125 out:
 126         return err;
 127 }
 128
 129 static int mptp_connect(struct socket *sock, struct sockaddr *addr, int addr_len, int flags)
 130 {
 131         int err;
 132         struct sock * sk;
 133         struct inet_sock * isk;
 134         struct mptp_sock * ssk;
 135
 136         log_debug("mptp_connect\n");
 137
 138         if (unlikely(sock == NULL)) {
 139                 log_error("Sock is NULL\n");
 140                 err = -EINVAL;
 141                 goto out;
 142         }
 143         sk = sock->sk;
 144
 145         if (unlikely(sk == NULL)) {
 146                 log_error("Sock->sk is NULL\n");
 147                 err = -EINVAL;
 148                 goto out;
 149         }
 150
 151         isk = inet_sk(sk);
 152         ssk = mptp_sk(sk);
 153
 154         if (unlikely(ssk->src != 0)) {
 155                 log_error("ssk->src is not NULL\n");
 156                 err = -EINVAL;
 157                 goto out;
 158         }
 159
 160         if (likely(addr)) {
 161                 struct sockaddr_mptp * mptp_addr = (struct sockaddr_mptp *) addr;
 162
 163         if (unlikely(addr_len < sizeof(*mptp_addr) ||
 164                      addr_len < mptp_addr->count * sizeof(struct mptp_dest) ||
 165                      mptp_addr->count <= 0)) {
 166                         log_error("Invalid size or address family\n");
 167                         err = -EINVAL;
 168                         goto out;
 169                 }
 170                 ssk->dst = ntohs(mptp_addr->dests[0].port);
 171                 if (unlikely(ssk->dst == 0 || ssk->dst >= MAX_MPTP_PORT)) {
 172                         log_error("Invalid value for destination port(%u)\n", ssk->dst);
 173                         err = -EINVAL;
 174                         goto out;
 175                 }
 176
 177                 isk->inet_daddr = mptp_addr->dests[0].addr;
 178                 log_debug("Received from user space destination port=%u and address=%u\n", ssk->dst, isk->inet_daddr);
 179         } else {
 180                 log_error("Invalid mptp_addr (NULL)\n");
 181                 err = -EINVAL;
 182                 goto out;
 183         }
 184
 185         ssk->src = get_next_free_port();
 186         if (unlikely(ssk->src == 0)) {
 187                 log_error("No free ports\n");
 188                 err = -ENOMEM;
 189                 goto out;
 190         }
 191
 192         mptp_hash(ssk->src, ssk);
 193
 194         return 0;
 195
 196 out:
 197         return err;
 198 }
 199
 200 static int mptp_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
 201 {
 202         int err;
 203         uint16_t dport;
 204     __be32 daddr;
 205     uint16_t sport;
 206     struct sk_buff * skb;
 207     struct sock * sk;
 208     struct inet_sock * isk;
 209     struct mptp_sock * ssk;
 210     struct mptphdr * shdr;
 211     int connected = 0;
 212     int totlen;
 213     struct rtable * rt = NULL;
 214     int dests = 0;
 215     int i;
 216     struct sockaddr_mptp * mptp_addr = NULL;
 217         int ret = 0;
 218
 219     if (unlikely(sock == NULL)) {
 220         log_error("Sock is NULL\n");
 221         err = -EINVAL;
 222         goto out;
 223     }
 224     sk = sock->sk;
 225
 226     if (unlikely(sk == NULL)) {
 227         log_error("Sock->sk is NULL\n");
 228         err = -EINVAL;
 229         goto out;
 230     }
 231
 232     isk = inet_sk(sk);
 233     ssk = mptp_sk(sk);
 234
 235     sport = ssk->src;
 236     if (sport == 0) {
 237         sport = get_next_free_port();
 238         if (unlikely(sport == 0)) {
 239             log_error("No free ports\n");
 240             err = -ENOMEM;
 241             goto out;
 242         }
 243     }
 244
 245     if (msg->msg_name) {
 246         mptp_addr = (struct sockaddr_mptp *) msg->msg_name;
 247
 248         if (unlikely(msg->msg_namelen < sizeof(*mptp_addr) + mptp_addr->count * sizeof(struct mptp_dest) ||
 249                      mptp_addr->count <= 0)) {
 250             log_error("Invalid size for msg_name (size=%u, addr_count=%u)\n", msg->msg_namelen, mptp_addr->count);
 251             err = -EINVAL;
 252             goto out;
 253         }
 254
 255         dests = mptp_addr->count;
 256     } else {
 257         BUG();
 258         if (unlikely(!ssk->dst || !isk->inet_daddr)) {
 259             log_error("No destination port/address\n");
 260             err = -EDESTADDRREQ;
 261             goto out;
 262         }
 263         dport = ssk->dst;
 264         daddr = isk->inet_daddr;
 265
 266         log_debug("Got from socket destination port=%u and address=%u\n", dport, daddr);
 267         connected = 1;
 268     }
 269
 270     if (msg->msg_iovlen < dests)
 271         dests = msg->msg_iovlen;
 272
 273     for (i = 0; i < dests; i++) {
 274         struct mptp_dest *dest = &mptp_addr->dests[i];
 275         struct iovec *iov = &msg->msg_iov[i];
 276         char *payload;
 277
 278         dport = ntohs(dest->port);
 279         if (unlikely(dport == 0 || dport >= MAX_MPTP_PORT)) {
 280             log_error("Invalid value for destination port(%u)\n", dport);
 281             err = -EINVAL;
 282             goto out;
 283         }
 284
 285         daddr = dest->addr;
 286         log_debug("Received from user space destination port=%u and address=%u\n", dport, daddr);
 287
 288         len = iov->iov_len;
 289         totlen = len + sizeof(struct mptphdr) + sizeof(struct iphdr);
 290         skb = sock_alloc_send_skb(sk, totlen, msg->msg_flags & MSG_DONTWAIT, &err);
 291         if (unlikely(!skb)) {
 292             log_error("sock_alloc_send_skb failed\n");
 293             goto out;
 294         }
 295         log_debug("Allocated %u bytes for skb (payload size=%u)\n", totlen, len);
 296
 297         skb_reset_network_header(skb);
 298         skb_reserve(skb, sizeof(struct iphdr));
 299         log_debug("Reseted network header\n");
 300         skb_reset_transport_header(skb);
 301         skb_put(skb, sizeof(struct mptphdr));
 302         log_debug("Reseted transport header\n");
 303
 304         shdr = (struct mptphdr *) skb_transport_header(skb);
 305         shdr->dst = htons(dport);
 306         shdr->src = htons(sport);
 307         shdr->len = htons(len + sizeof(struct mptphdr));
 308
 309         payload = skb_put(skb, len);
 310         log_debug("payload=%p\n", payload);
 311
 312         err = skb_copy_datagram_from_iovec(skb, sizeof(struct mptphdr), iov, 0, len);
 313         if (unlikely(err)) {
 314             log_error("skb_copy_datagram_from_iovec failed\n");
 315             goto out_free;
 316         }
 317         log_debug("Copied %u bytes into the skb\n", len);
 318
 319         if (connected)
 320             rt = (struct rtable *) __sk_dst_check(sk, 0);
 321
 322         if (rt == NULL) {
 323             struct flowi fl = { .fl4_dst = daddr,
 324                 .proto = sk->sk_protocol,
 325                 .flags = inet_sk_flowi_flags(sk),
 326             };
 327             err = ip_route_output_flow(sock_net(sk), &rt, &fl, sk, 0);
 328             if (unlikely(err)) {
 329                 log_error("Route lookup failed\n");
 330                 goto out_free;
 331             }
 332 #if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36)
 333             sk_dst_set(sk, dst_clone(&rt->u.dst));
 334 #else
 335             sk_dst_set(sk, dst_clone(&rt->dst));
 336 #endif
 337         }
 338
 339         skb->local_df = 1;
 340         err = ip_queue_xmit(skb);
 341         if (likely(!err)) {
 342             log_debug("Sent %u bytes on wire\n", len);
 343                         ret += len;
 344                         dest->bytes = len;
 345                 } else {
 346             log_error("ip_queue_xmit failed\n");
 347                         dest->bytes = -1;
 348                 }
 349     }
 350
 351         return ret;
 352
 353 out_free:
 354         kfree(skb);
 355
 356 out:
 357         return err;
 358 }
 359
 360 static int mptp_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
 361 {
 362         struct sk_buff *skb;
 363         struct sockaddr_mptp *mptp_addr;
 364         struct sock * sk = sock->sk;
 365         int err, copied;
 366         int i;
 367         struct sockaddr_mptp *ret_addr = (struct sockaddr_mptp *) msg->msg_name;
 368         ret_addr->count = 0;
 369
 370     log_debug("Trying to receive sock=%p sk=%p flags=%d\n", sock, sk, flags);
 371
 372         skb = skb_recv_datagram(sk, flags, flags & MSG_DONTWAIT, &err);
 373         if (unlikely(!skb)) {
 374                 log_error("skb_recv_datagram failed with %d\n", err);
 375                 goto out;
 376         }
 377
 378         for (i = 0; i < msg->msg_iovlen; i++) {
 379                 log_debug("Received skb %p\n", skb);
 380
 381                 mptp_addr = (struct sockaddr_mptp *) skb->cb;
 382
 383                 copied = skb->len;
 384                 if (copied > msg->msg_iov[i].iov_len) {
 385                         copied = msg->msg_iov[i].iov_len;
 386                         msg->msg_flags |= MSG_TRUNC;
 387                 }
 388
 389                 err = skb_copy_datagram_iovec(skb, 0, &msg->msg_iov[i], copied);
 390                 if (unlikely(err)) {
 391                         log_error("skb_copy_datagram_iovec\n");
 392                         goto out_free;
 393                 }
 394                 log_debug("Received %d bytes\n", copied);
 395
 396                 sock_recv_ts_and_drops(msg, sk, skb);
 397
 398                 if (ret_addr) {
 399                         memcpy(&ret_addr->dests[i], &mptp_addr->dests[0], sizeof(ret_addr->dests[i]));
 400                         ret_addr->dests[i].bytes = copied;
 401                 }
 402
 403                 err = copied;
 404
 405 out_free:
 406             skb_free_datagram(sk, skb);
 407
 408                 if (i == msg->msg_iovlen - 1)
 409                         break;
 410
 411                 skb = skb_recv_datagram(sk, flags, 1, &err);
 412                 if (likely(err == -EAGAIN)) {
 413                         log_debug("No more skbs in the queue, returning...\n");
 414                         err = copied;
 415                         break;
 416                 }
 417         }
 418
 419         ret_addr->count = i + 1;
 420         msg->msg_namelen = sizeof(struct sockaddr_mptp) + (i + 1) * sizeof(struct mptp_dest);
 421
 422 out:
 423         return err;
 424 }
 425
 426 static int mptp_rcv(struct sk_buff *skb)
 427 {
 428         struct mptphdr *shdr;
 429         struct mptp_sock *ssk;
 430         __be16 len;
 431         uint16_t src, dst;
 432         struct sockaddr_mptp * mptp_addr;
 433         int err;
 434         int addr_size = sizeof(struct sockaddr_mptp) + sizeof(struct mptp_dest);
 435
 436         if (unlikely(!pskb_may_pull(skb, sizeof(struct mptphdr)))) {
 437                 log_error("Insufficient space for header\n");
 438                 goto drop;
 439         }
 440
 441         shdr = (struct mptphdr *) skb->data;
 442         len = ntohs(shdr->len);
 443
 444         if (unlikely(skb->len < len)) {
 445                 log_error("Malformed packet (packet_len=%u, skb_len=%u)\n", len, skb->len);
 446                 goto drop;
 447         }
 448
 449         if (unlikely(len < sizeof(struct mptphdr))) {
 450                 log_error("Malformed packet (packet_len=%u sizeof(mptphdr)=%u\n", len, sizeof(struct mptphdr));
 451                 goto drop;
 452         }
 453
 454         src = ntohs(shdr->src);
 455         dst = ntohs(shdr->dst);
 456         if (unlikely(src == 0 || dst == 0 || src >= MAX_MPTP_PORT || dst >= MAX_MPTP_PORT)) {
 457                 log_error("Malformed packet (src=%u, dst=%u)\n", shdr->src, shdr->dst);
 458                 goto drop;
 459         }
 460
 461         skb_pull(skb, sizeof(struct mptphdr));
 462         len -= sizeof(struct mptphdr);
 463
 464         pskb_trim(skb, len);
 465
 466         log_debug("Received %u bytes from from port=%u to port=%u\n", len - sizeof(struct mptphdr), src, dst);
 467
 468         ssk = mptp_lookup(dst);
 469         if (ssk == NULL) {
 470                 log_error("MPTP lookup failed for port %u\n", dst);
 471                 goto drop;
 472         }
 473
 474         BUG_ON(addr_size > sizeof(skb->cb));
 475
 476         mptp_addr = (struct sockaddr_mptp *) skb->cb;
 477         mptp_addr->dests[0].port = shdr->src;
 478         mptp_addr->dests[0].addr = ip_hdr(skb)->saddr;
 479
 480         log_debug("Setting sin_port=%u, sin_addr=%u\n", ntohs(shdr->src), mptp_addr->dests[0].addr);
 481
 482         err = ip_queue_rcv_skb((struct sock *) &ssk->sock, skb);
 483         if (unlikely(err)) {
 484                 log_error("ip_queue_rcv_skb failed with %d\n", err);
 485                 consume_skb(skb);
 486         }
 487         return NET_RX_SUCCESS;
 488
 489 drop:
 490         kfree(skb);
 491         return NET_RX_DROP;
 492 }
 493
 494 static struct proto mptp_prot = {
 495         .obj_size = sizeof(struct mptp_sock),
 496         .owner    = THIS_MODULE,
 497         .name     = "MPTP",
 498 };
 499
 500 static const struct proto_ops mptp_ops = {
 501         .family     = PF_INET,
 502         .owner      = THIS_MODULE,
 503         .release    = mptp_release,
 504         .bind       = mptp_bind,
 505         .connect    = mptp_connect,
 506         .socketpair = sock_no_socketpair,
 507         .accept     = sock_no_accept,
 508         .getname    = sock_no_getname,
 509         .poll       = datagram_poll,
 510         .ioctl      = sock_no_ioctl,
 511         .listen     = sock_no_listen,
 512         .shutdown   = sock_no_shutdown,
 513         .setsockopt = sock_no_setsockopt,
 514         .getsockopt = sock_no_getsockopt,
 515         .sendmsg    = mptp_sendmsg,
 516         .recvmsg    = mptp_recvmsg,
 517         .mmap       = sock_no_mmap,
 518         .sendpage   = sock_no_sendpage,
 519 };
 520
 521 static const struct net_protocol mptp_protocol = {
 522         .handler   = mptp_rcv,
 523         .no_policy = 1,
 524         .netns_ok  = 1,
 525 };
 526
 527 static struct inet_protosw mptp_protosw = {
 528         .type     = SOCK_DGRAM,
 529         .protocol = IPPROTO_MPTP,
 530         .prot     = &mptp_prot,
 531         .ops      = &mptp_ops,
 532         .no_check = 0,
 533 };
 534
 535 static int __init mptp_init(void)
 536 {
 537         int rc;
 538
 539         rc = proto_register(&mptp_prot, 1);
 540         if (unlikely(rc)) {
 541                 log_error("Error registering mptp protocol\n");
 542                 goto out;
 543         }
 544
 545         rc = inet_add_protocol(&mptp_protocol, IPPROTO_MPTP);
 546         if (unlikely(rc)) {
 547                 log_error("Error adding mptp protocol\n");
 548                 goto out_unregister;
 549         }
 550
 551         inet_register_protosw(&mptp_protosw);
 552         log_debug("MPTP entered\n");
 553
 554         return 0;
 555
 556 out_unregister:
 557         proto_unregister(&mptp_prot);
 558
 559 out:
 560         return rc;
 561 }
 562
 563 static void __exit mptp_exit(void)
 564 {
 565         inet_unregister_protosw(&mptp_protosw);
 566
 567         inet_del_protocol(&mptp_protocol, IPPROTO_MPTP);
 568
 569         proto_unregister(&mptp_prot);
 570
 571         log_debug("MPTP exited\n");
 572 }
 573
 574 module_init(mptp_init);
 575 module_exit(mptp_exit);