src/kernel/swift.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 "swift.h"
   9 #include "debug.h"
  10
  11 MODULE_DESCRIPTION("Swift Transport Protocol");
  12 MODULE_AUTHOR("Adrian Bondrescu/Cornel Mercan");
  13 MODULE_LICENSE("GPL");
  14
  15 struct swift_sock {
  16         struct inet_sock sock;
  17         /* swift socket speciffic data */
  18         uint8_t src;
  19         uint8_t dst;
  20 };
  21
  22 static struct swift_sock * sock_port_map[MAX_SWIFT_PORT];
  23
  24 static inline struct swift_sock * swift_sk(struct sock * sock)
  25 {
  26         return (struct swift_sock *)(sock);
  27 }
  28
  29 static inline struct swifthdr * swift_hdr(const struct sk_buff * skb)
  30 {
  31         return (struct swifthdr *) skb_transport_header(skb);
  32 }
  33
  34 static inline uint8_t get_next_free_port(void)
  35 {
  36         int i;
  37         for (i = MIN_SWIFT_PORT; i < MAX_SWIFT_PORT; i ++)
  38                 if (sock_port_map[i] == NULL)
  39                         return i;
  40         return 0;
  41 }
  42
  43 static inline void swift_unhash(uint8_t port)
  44 {
  45         sock_port_map[port] = NULL;
  46 }
  47
  48 static inline void swift_hash(uint8_t port, struct swift_sock *ssh)
  49 {
  50         sock_port_map[port] = ssh;
  51 }
  52
  53 static inline struct swift_sock * swift_lookup(uint8_t port)
  54 {
  55         return sock_port_map[port];
  56 }
  57
  58 static int swift_release(struct socket *sock)
  59 {
  60         struct sock *sk = sock->sk;
  61         struct swift_sock * ssk = swift_sk(sk);
  62
  63         if (unlikely(!sk))
  64                 return 0;
  65
  66         swift_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("swift_release sock=%p\n", sk);
  78         sock_put(sk);
  79
  80         return 0;
  81 }
  82
  83 static int swift_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
  84 {
  85         struct sockaddr_swift *swift_addr;
  86         struct swift_sock *ssk;
  87         int err;
  88         uint8_t port;
  89
  90         if (unlikely(addr_len < sizeof(struct sockaddr_swift))) {
  91                 log_error("Invalid size for sockaddr\n");
  92                 err = -EINVAL;
  93                 goto out;
  94         }
  95
  96         swift_addr = (struct sockaddr_swift *) addr;
  97
  98         port = swift_addr->dests[0].port;
  99
 100         if (unlikely(port == 0 || port >= MAX_SWIFT_PORT)) {
 101                 log_error("Invalid value for sockaddr port (%u)\n", port);
 102                 err = -EINVAL;
 103                 goto out;
 104         }
 105
 106         if (unlikely(swift_lookup(port) != NULL)) {
 107                 log_error("Port %u already in use\n", port);
 108                 err = -EADDRINUSE;
 109                 goto out;
 110         }
 111
 112         ssk = swift_sk(sock->sk);
 113     sock->sk->sk_rcvbuf = 10 * 1024 * 1024;
 114         ssk->src = port;
 115
 116         swift_hash(port, ssk);
 117
 118         log_debug("Socket %p bound to port %u\n", ssk, port);
 119
 120         return 0;
 121
 122 out:
 123         return err;
 124 }
 125
 126 static int swift_connect(struct socket *sock, struct sockaddr *addr, int addr_len, int flags)
 127 {
 128         int err;
 129         struct sock * sk;
 130         struct inet_sock * isk;
 131         struct swift_sock * ssk;
 132
 133         log_debug("swift_connect\n");
 134
 135         if (unlikely(sock == NULL)) {
 136                 log_error("Sock is NULL\n");
 137                 err = -EINVAL;
 138                 goto out;
 139         }
 140         sk = sock->sk;
 141
 142         if (unlikely(sk == NULL)) {
 143                 log_error("Sock->sk is NULL\n");
 144                 err = -EINVAL;
 145                 goto out;
 146         }
 147
 148         isk = inet_sk(sk);
 149         ssk = swift_sk(sk);
 150
 151         if (unlikely(ssk->src != 0)) {
 152                 log_error("ssk->src is not NULL\n");
 153                 err = -EINVAL;
 154                 goto out;
 155         }
 156
 157         if (likely(addr)) {
 158                 struct sockaddr_swift * swift_addr = (struct sockaddr_swift *) addr;
 159
 160         if (unlikely(addr_len < sizeof(*swift_addr) ||
 161                      addr_len < swift_addr->count * sizeof(struct swift_dest) ||
 162                      swift_addr->count <= 0)) {
 163                         log_error("Invalid size or address family\n");
 164                         err = -EINVAL;
 165                         goto out;
 166                 }
 167                 ssk->dst = swift_addr->dests[0].port;
 168                 if (unlikely(ssk->dst == 0 || ssk->dst >= MAX_SWIFT_PORT)) {
 169                         log_error("Invalid value for destination port(%u)\n", ssk->dst);
 170                         err = -EINVAL;
 171                         goto out;
 172                 }
 173
 174                 isk->inet_daddr = swift_addr->dests[0].addr;
 175                 log_debug("Received from user space destination port=%u and address=%u\n", ssk->dst, isk->inet_daddr);
 176         } else {
 177                 log_error("Invalid swift_addr (NULL)\n");
 178                 err = -EINVAL;
 179                 goto out;
 180         }
 181
 182         ssk->src = get_next_free_port();
 183         if (unlikely(ssk->src == 0)) {
 184                 log_error("No free ports\n");
 185                 err = -ENOMEM;
 186                 goto out;
 187         }
 188
 189         swift_hash(ssk->src, ssk);
 190
 191         return 0;
 192
 193 out:
 194         return err;
 195 }
 196
 197 static int swift_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
 198 {
 199         int err;
 200         uint8_t dport;
 201     __be32 daddr;
 202     uint8_t sport;
 203     struct sk_buff * skb;
 204     struct sock * sk;
 205     struct inet_sock * isk;
 206     struct swift_sock * ssk;
 207     struct swifthdr * shdr;
 208     int connected = 0;
 209     int totlen;
 210     struct rtable * rt = NULL;
 211     int dests = 0;
 212     int i;
 213     struct sockaddr_swift * swift_addr = NULL;
 214
 215     if (unlikely(sock == NULL)) {
 216         log_error("Sock is NULL\n");
 217         err = -EINVAL;
 218         goto out;
 219     }
 220     sk = sock->sk;
 221
 222     if (unlikely(sk == NULL)) {
 223         log_error("Sock->sk is NULL\n");
 224         err = -EINVAL;
 225         goto out;
 226     }
 227
 228     isk = inet_sk(sk);
 229     ssk = swift_sk(sk);
 230
 231     sport = ssk->src;
 232     if (sport == 0) {
 233         sport = get_next_free_port();
 234         if (unlikely(sport == 0)) {
 235             log_error("No free ports\n");
 236             err = -ENOMEM;
 237             goto out;
 238         }
 239     }
 240
 241     if (msg->msg_name) {
 242         swift_addr = (struct sockaddr_swift *) msg->msg_name;
 243
 244         if (unlikely(msg->msg_namelen < sizeof(*swift_addr) ||
 245                      msg->msg_namelen < swift_addr->count * sizeof(struct swift_dest) ||
 246                      swift_addr->count <= 0)) {
 247             log_error("Invalid size for msg_name\n");
 248             err = -EINVAL;
 249             goto out;
 250         }
 251
 252         dests = swift_addr->count;
 253     } else {
 254         BUG();
 255         if (unlikely(!ssk->dst || !isk->inet_daddr)) {
 256             log_error("No destination port/address\n");
 257             err = -EDESTADDRREQ;
 258             goto out;
 259         }
 260         dport = ssk->dst;
 261         daddr = isk->inet_daddr;
 262
 263         log_debug("Got from socket destination port=%u and address=%u\n", dport, daddr);
 264         connected = 1;
 265     }
 266
 267     if (msg->msg_iovlen < dests)
 268         dests = msg->msg_iovlen;
 269
 270     for (i = 0; i < dests; i++) {
 271         struct swift_dest *dest = &swift_addr->dests[i];
 272         struct iovec *iov = &msg->msg_iov[i];
 273         char *payload;
 274
 275         dport = dest->port;
 276         if (unlikely(dport == 0 || dport >= MAX_SWIFT_PORT)) {
 277             log_error("Invalid value for destination port(%u)\n", dport);
 278             err = -EINVAL;
 279             goto out;
 280         }
 281
 282         daddr = dest->addr;
 283         log_debug("Received from user space destination port=%u and address=%u\n", dport, daddr);
 284
 285         len = iov->iov_len;
 286         totlen = len + sizeof(struct swifthdr) + sizeof(struct iphdr);
 287         skb = sock_alloc_send_skb(sk, totlen, msg->msg_flags & MSG_DONTWAIT, &err);
 288         if (unlikely(!skb)) {
 289             log_error("sock_alloc_send_skb failed\n");
 290             goto out;
 291         }
 292         log_debug("Allocated %u bytes for skb (payload size=%u)\n", totlen, len);
 293
 294         skb_reset_network_header(skb);
 295         skb_reserve(skb, sizeof(struct iphdr));
 296         log_debug("Reseted network header\n");
 297         skb_reset_transport_header(skb);
 298         skb_put(skb, sizeof(struct swifthdr));
 299         log_debug("Reseted transport header\n");
 300
 301         shdr = (struct swifthdr *) skb_transport_header(skb);
 302         shdr->dst = dport;
 303         shdr->src = sport;
 304         shdr->len = ntohs(len + sizeof(struct swifthdr));
 305
 306         payload = skb_put(skb, len);
 307         log_debug("payload=%p\n", payload);
 308
 309         err = skb_copy_datagram_from_iovec(skb, sizeof(struct swifthdr), iov, 0, len);
 310         if (unlikely(err)) {
 311             log_error("skb_copy_datagram_from_iovec failed\n");
 312             goto out_free;
 313         }
 314         log_debug("Copied %u bytes into the skb\n", len);
 315
 316         if (connected)
 317             rt = (struct rtable *) __sk_dst_check(sk, 0);
 318
 319         if (rt == NULL) {
 320             struct flowi fl = { .fl4_dst = daddr,
 321                 .proto = sk->sk_protocol,
 322                 .flags = inet_sk_flowi_flags(sk),
 323             };
 324             err = ip_route_output_flow(sock_net(sk), &rt, &fl, sk, 0);
 325             if (unlikely(err)) {
 326                 log_error("Route lookup failed\n");
 327                 goto out_free;
 328             }
 329 #if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36)
 330             sk_dst_set(sk, dst_clone(&rt->u.dst));
 331 #else
 332             sk_dst_set(sk, dst_clone(&rt->dst));
 333 #endif
 334         }
 335
 336         skb->local_df = 1;
 337         err = ip_queue_xmit(skb);
 338         if (likely(!err))
 339             log_debug("Sent %u bytes on wire\n", len);
 340         else
 341             log_error("ip_queue_xmit failed\n");
 342     }
 343
 344         return err;
 345
 346 out_free:
 347         kfree(skb);
 348
 349 out:
 350         return err;
 351 }
 352
 353 static int swift_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
 354 {
 355         struct sk_buff *skb;
 356         struct sockaddr_swift *swift_addr;
 357         struct sock * sk = sock->sk;
 358         int err, copied;
 359         int i;
 360         struct sockaddr_swift *ret_addr = (struct sockaddr_swift *) msg->msg_name;
 361
 362     log_debug("Trying to receive sock=%p sk=%p flags=%d\n", sock, sk, flags);
 363
 364         skb = skb_recv_datagram(sk, flags, flags & MSG_DONTWAIT, &err);
 365         if (unlikely(!skb)) {
 366                 log_error("skb_recv_datagram failed with %d\n", err);
 367                 goto out;
 368         }
 369
 370         for (i = 0; i < msg->msg_iovlen; i++) {
 371                 log_debug("Received skb %p\n", skb);
 372
 373                 swift_addr = (struct sockaddr_swift *) skb->cb;
 374
 375                 copied = skb->len;
 376                 if (copied > msg->msg_iov[i].iov_len) {
 377                         copied = msg->msg_iov[i].iov_len;
 378                         msg->msg_flags |= MSG_TRUNC;
 379                 }
 380
 381                 err = skb_copy_datagram_iovec(skb, 0, &msg->msg_iov[i], copied);
 382                 if (unlikely(err)) {
 383                         log_error("skb_copy_datagram_iovec\n");
 384                         goto out_free;
 385                 }
 386                 log_debug("Received %d bytes\n", copied);
 387
 388                 sock_recv_ts_and_drops(msg, sk, skb);
 389
 390                 if (ret_addr) {
 391                         memcpy(&ret_addr->dests[i], &mptp_addr->dests[0], sizeof(ret_addr->dests[i]));
 392                         ret_addr->dests[i].bytes = copied;
 393                 }
 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_swift) + (i + 1) * sizeof(struct swift_dest);
 413
 414 out:
 415         return err;
 416 }
 417
 418 static int swift_rcv(struct sk_buff *skb)
 419 {
 420         struct swifthdr *shdr;
 421         struct swift_sock *ssk;
 422         __be16 len;
 423         uint8_t src, dst;
 424         struct sockaddr_swift * swift_addr;
 425         int err;
 426         int addr_size = sizeof(struct sockaddr_swift) + sizeof(struct swift_dest);
 427
 428         if (unlikely(!pskb_may_pull(skb, sizeof(struct swifthdr)))) {
 429                 log_error("Insufficient space for header\n");
 430                 goto drop;
 431         }
 432
 433         shdr = (struct swifthdr *) 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 swifthdr))) {
 442                 log_error("Malformed packet (packet_len=%u sizeof(swifthdr)=%u\n", len, sizeof(struct swifthdr));
 443                 goto drop;
 444         }
 445
 446         src = shdr->src;
 447         dst = shdr->dst;
 448         if (unlikely(src == 0 || dst == 0 || src >= MAX_SWIFT_PORT || dst >= MAX_SWIFT_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 swifthdr));
 454         len -= sizeof(struct swifthdr);
 455
 456         pskb_trim(skb, len);
 457
 458         log_debug("Received %u bytes from from port=%u to port=%u\n", len - sizeof(struct swifthdr), src, dst);
 459
 460         ssk = swift_lookup(dst);
 461         if (ssk == NULL) {
 462                 log_error("Swift lookup failed for port %u\n", dst);
 463                 goto drop;
 464         }
 465
 466         BUG_ON(addr_size > sizeof(skb->cb));
 467
 468         swift_addr = (struct sockaddr_swift *) skb->cb;
 469         swift_addr->dests[0].port = shdr->src;
 470         swift_addr->dests[0].addr = ip_hdr(skb)->saddr;
 471
 472         log_debug("Setting sin_port=%u, sin_addr=%u\n", ntohs(shdr->src), swift_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 swift_prot = {
 487         .obj_size = sizeof(struct swift_sock),
 488         .owner    = THIS_MODULE,
 489         .name     = "SWIFT",
 490 };
 491
 492 static const struct proto_ops swift_ops = {
 493         .family     = PF_INET,
 494         .owner      = THIS_MODULE,
 495         .release    = swift_release,
 496         .bind       = swift_bind,
 497         .connect    = swift_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    = swift_sendmsg,
 508         .recvmsg    = swift_recvmsg,
 509         .mmap       = sock_no_mmap,
 510         .sendpage   = sock_no_sendpage,
 511 };
 512
 513 static const struct net_protocol swift_protocol = {
 514         .handler   = swift_rcv,
 515         .no_policy = 1,
 516         .netns_ok  = 1,
 517 };
 518
 519 static struct inet_protosw swift_protosw = {
 520         .type     = SOCK_DGRAM,
 521         .protocol = IPPROTO_SWIFT,
 522         .prot     = &swift_prot,
 523         .ops      = &swift_ops,
 524         .no_check = 0,
 525 };
 526
 527 static int __init swift_init(void)
 528 {
 529         int rc;
 530
 531         rc = proto_register(&swift_prot, 1);
 532         if (unlikely(rc)) {
 533                 log_error("Error registering swift protocol\n");
 534                 goto out;
 535         }
 536
 537         rc = inet_add_protocol(&swift_protocol, IPPROTO_SWIFT);
 538         if (unlikely(rc)) {
 539                 log_error("Error adding swift protocol\n");
 540                 goto out_unregister;
 541         }
 542
 543         inet_register_protosw(&swift_protosw);
 544         log_debug("Swift entered\n");
 545
 546         return 0;
 547
 548 out_unregister:
 549         proto_unregister(&swift_prot);
 550
 551 out:
 552         return rc;
 553 }
 554
 555 static void __exit swift_exit(void)
 556 {
 557         inet_unregister_protosw(&swift_protosw);
 558
 559         inet_del_protocol(&swift_protocol, IPPROTO_SWIFT);
 560
 561         proto_unregister(&swift_prot);
 562
 563         log_debug("Swift exited\n");
 564 }
 565
 566 module_init(swift_init);
 567 module_exit(swift_exit);