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