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         if (unlikely(swift_addr->sin_family != AF_INET)) {
  99                 log_error("Invalid family for sockaddr\n");
 100                 err = -EINVAL;
 101                 goto out;
 102         }
 103
 104         port = ntohs(swift_addr->sin_port);
 105
 106         if (unlikely(port == 0 || port >= MAX_SWIFT_PORT)) {
 107                 log_error("Invalid value for sockaddr port (%u)\n", port);
 108                 err = -EINVAL;
 109                 goto out;
 110         }
 111
 112         if (unlikely(swift_lookup(port) != NULL)) {
 113                 log_error("Port %u already in use\n", port);
 114                 err = -EADDRINUSE;
 115                 goto out;
 116         }
 117
 118         ssk = swift_sk(sock->sk);
 119         ssk->src = port;
 120
 121         swift_hash(port, ssk);
 122
 123         log_debug("Socket %p bound to port %u\n", ssk, port);
 124
 125         return 0;
 126
 127 out:
 128         return err;
 129 }
 130
 131 static int swift_connect(struct socket *sock, struct sockaddr *addr, int addr_len, int flags)
 132 {
 133         int err;
 134         struct sock * sk;
 135         struct inet_sock * isk;
 136         struct swift_sock * ssk;
 137
 138         log_debug("swift_connect\n");
 139
 140         if (unlikely(sock == NULL)) {
 141                 log_error("Sock is NULL\n");
 142                 err = -EINVAL;
 143                 goto out;
 144         }
 145         sk = sock->sk;
 146
 147         if (unlikely(sk == NULL)) {
 148                 log_error("Sock->sk is NULL\n");
 149                 err = -EINVAL;
 150                 goto out;
 151         }
 152
 153         isk = inet_sk(sk);
 154         ssk = swift_sk(sk);
 155
 156         if (unlikely(ssk->src != 0)) {
 157                 log_error("ssk->src is not NULL\n");
 158                 err = -EINVAL;
 159                 goto out;
 160         }
 161
 162         if (likely(addr)) {
 163                 struct sockaddr_swift * swift_addr = (struct sockaddr_swift *) addr;
 164
 165                 if (unlikely(addr_len < sizeof(*swift_addr) || swift_addr->sin_family != AF_INET)) {
 166                         log_error("Invalid size or address family\n");
 167                         err = -EINVAL;
 168                         goto out;
 169                 }
 170                 ssk->dst = ntohs(swift_addr->sin_port);
 171                 if (unlikely(ssk->dst == 0 || ssk->dst >= MAX_SWIFT_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 = swift_addr->sin_addr.s_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 swift_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         swift_hash(ssk->src, ssk);
 193
 194         return 0;
 195
 196 out:
 197         return err;
 198 }
 199
 200 static int swift_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
 201 {
 202         int err;
 203         uint8_t dport;
 204         __be32 daddr;
 205         uint8_t sport;
 206         struct sk_buff * skb;
 207         struct sock * sk;
 208         struct inet_sock * isk;
 209         struct swift_sock * ssk;
 210         struct swifthdr * shdr;
 211         int connected = 0;
 212         int totlen;
 213         struct rtable * rt = 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                 struct sockaddr_swift * swift_addr = (struct sockaddr_swift *) msg->msg_name;
 243
 244                 if (unlikely(msg->msg_namelen < sizeof(*swift_addr) || swift_addr->sin_family != AF_INET)) {
 245                         log_error("Invalid size or address family\n");
 246                         err = -EINVAL;
 247                         goto out;
 248                 }
 249
 250                 dport = ntohs(swift_addr->sin_port);
 251                 if (unlikely(dport == 0 || dport >= MAX_SWIFT_PORT)) {
 252                         log_error("Invalid value for destination port(%u)\n", dport);
 253                         err = -EINVAL;
 254                         goto out;
 255                 }
 256
 257                 daddr = swift_addr->sin_addr.s_addr;
 258                 log_debug("Received from user space destination port=%u and address=%u\n", dport, daddr);
 259         } else {
 260                 if (unlikely(!ssk->dst || !isk->inet_daddr)) {
 261                         log_error("No destination port/address\n");
 262                         err = -EDESTADDRREQ;
 263                         goto out;
 264                 }
 265                 dport = ssk->dst;
 266                 daddr = isk->inet_daddr;
 267
 268                 log_debug("Got from socket destination port=%u and address=%u\n", dport, daddr);
 269                 connected = 1;
 270         }
 271
 272         totlen = len + sizeof(struct swifthdr) + sizeof(struct iphdr);
 273         skb = sock_alloc_send_skb(sk, totlen, msg->msg_flags & MSG_DONTWAIT, &err);
 274         if (unlikely(!skb)) {
 275                 log_error("sock_alloc_send_skb failed\n");
 276                 goto out;
 277         }
 278         log_debug("Allocated %u bytes for skb (payload size=%u)\n", totlen, len);
 279
 280         skb_reset_network_header(skb);
 281         skb_reserve(skb, sizeof(struct iphdr));
 282         log_debug("Reseted network header\n");
 283         skb_reset_transport_header(skb);
 284         skb_put(skb, sizeof(struct swifthdr));
 285         log_debug("Reseted transport header\n");
 286
 287         shdr = (struct swifthdr *) skb_transport_header(skb);
 288         shdr->dst = ntohs(dport);
 289         shdr->src = ntohs(sport);
 290         shdr->len = ntohs(len + sizeof(struct swifthdr));
 291
 292         log_debug("payload=%p\n", skb_put(skb, len));
 293
 294         err = skb_copy_datagram_from_iovec(skb, sizeof(struct swifthdr), msg->msg_iov, 0, len);
 295         if (unlikely(err)) {
 296                 log_error("skb_copy_datagram_from_iovec failed\n");
 297                 goto out_free;
 298         }
 299         log_debug("Copied %u bytes into the skb\n", len);
 300
 301         if (connected)
 302                 rt = (struct rtable *) __sk_dst_check(sk, 0);
 303
 304         if (rt == NULL) {
 305                 struct flowi fl = { .fl4_dst = daddr,
 306                                     .proto = sk->sk_protocol,
 307                                     .flags = inet_sk_flowi_flags(sk),
 308                                   };
 309                 err = ip_route_output_flow(sock_net(sk), &rt, &fl, sk, 0);
 310                 if (unlikely(err)) {
 311                         log_error("Route lookup failed\n");
 312                         goto out_free;
 313                 }
 314 #if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36)
 315                 sk_dst_set(sk, dst_clone(&rt->u.dst));
 316 #else
 317                 sk_dst_set(sk, dst_clone(&rt->dst));
 318 #endif
 319         }
 320
 321         err = ip_queue_xmit(skb);
 322         if (likely(!err))
 323                 log_debug("Sent %u bytes on wire\n", len);
 324         else
 325                 log_error("ip_queue_xmit failed\n");
 326
 327         return err;
 328
 329 out_free:
 330         kfree(skb);
 331
 332 out:
 333         return err;
 334 }
 335
 336 static int swift_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
 337 {
 338         struct sk_buff *skb;
 339         struct sockaddr_swift *swift_addr;
 340         struct sock * sk = sock->sk;
 341         int err, copied;
 342
 343         skb = skb_recv_datagram(sk, flags, flags & MSG_DONTWAIT, &err);
 344         if (unlikely(!skb)) {
 345                 log_error("skb_recv_datagram\n");
 346                 goto out;
 347         }
 348
 349         log_debug("Received skb %p\n", skb);
 350
 351         swift_addr = (struct sockaddr_swift *) skb->cb;
 352         msg->msg_namelen = sizeof(struct sockaddr_swift);
 353
 354         copied = skb->len;
 355         if (copied > len) {
 356                 copied = len;
 357                 msg->msg_flags |= MSG_TRUNC;
 358         }
 359
 360         err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
 361         if (unlikely(err)) {
 362                 log_error("skb_copy_datagram_iovec\n");
 363                 goto out_free;
 364         }
 365
 366         sock_recv_ts_and_drops(msg, sk, skb);
 367
 368         if (msg->msg_name)
 369                 memcpy(msg->msg_name, swift_addr, msg->msg_namelen);
 370
 371         err = copied;
 372
 373 out_free:
 374         skb_free_datagram(sk, skb);
 375
 376 out:
 377         return err;
 378 }
 379
 380 static int swift_rcv(struct sk_buff *skb)
 381 {
 382         struct swifthdr *shdr;
 383         struct swift_sock *ssk;
 384         __be16 len;
 385         uint8_t src, dst;
 386         struct sockaddr_swift * swift_addr;
 387         int err;
 388
 389         if (unlikely(!pskb_may_pull(skb, sizeof(struct swifthdr)))) {
 390                 log_error("Insufficient space for header\n");
 391                 goto drop;
 392         }
 393
 394         shdr = (struct swifthdr *) skb->data;
 395         len = ntohs(shdr->len);
 396
 397         if (unlikely(skb->len < len)) {
 398                 log_error("Malformed packet (packet_len=%u, skb_len=%u)\n", len, skb->len);
 399                 goto drop;
 400         }
 401
 402         if (unlikely(len < sizeof(struct swifthdr))) {
 403                 log_error("Malformed packet (packet_len=%u sizeof(swifthdr)=%u\n", len, sizeof(struct swifthdr));
 404                 goto drop;
 405         }
 406
 407         src = ntohs(shdr->src);
 408         dst = ntohs(shdr->dst);
 409         if (unlikely(src == 0 || dst == 0 || src >= MAX_SWIFT_PORT || dst >= MAX_SWIFT_PORT)) {
 410                 log_error("Malformed packet (src=%u, dst=%u)\n", shdr->src, shdr->dst);
 411                 goto drop;
 412         }
 413
 414         skb_pull(skb, sizeof(struct swifthdr));
 415         len -= sizeof(struct swifthdr);
 416
 417         pskb_trim(skb, len);
 418
 419         log_debug("Received %u bytes from from port=%u to port=%u\n", len - sizeof(struct swifthdr), src, dst);
 420
 421         ssk = swift_lookup(dst);
 422         if (ssk == NULL) {
 423                 log_error("Swift lookup failed for port %u\n", dst);
 424                 goto drop;
 425         }
 426
 427         BUILD_BUG_ON(sizeof(struct sockaddr_swift) > sizeof(skb->cb));
 428
 429         swift_addr = (struct sockaddr_swift *) skb->cb;
 430         swift_addr->sin_family = AF_INET;
 431         swift_addr->sin_port = shdr->src;
 432         swift_addr->sin_addr.s_addr = ip_hdr(skb)->saddr;
 433
 434         log_debug("Setting sin_port=%u, sin_addr=%u\n", ntohs(shdr->src), swift_addr->sin_addr.s_addr);
 435
 436         err = ip_queue_rcv_skb((struct sock *) &ssk->sock, skb);
 437         if (unlikely(err)) {
 438                 log_error("ip_queu_rcv_skb\n");
 439                 consume_skb(skb);
 440         }
 441         return NET_RX_SUCCESS;
 442
 443 drop:
 444         kfree(skb);
 445         return NET_RX_DROP;
 446 }
 447
 448 static struct proto swift_prot = {
 449         .obj_size = sizeof(struct swift_sock),
 450         .owner    = THIS_MODULE,
 451         .name     = "SWIFT",
 452 };
 453
 454 static const struct proto_ops swift_ops = {
 455         .family     = PF_INET,
 456         .owner      = THIS_MODULE,
 457         .release    = swift_release,
 458         .bind       = swift_bind,
 459         .connect    = swift_connect,
 460         .socketpair = sock_no_socketpair,
 461         .accept     = sock_no_accept,
 462         .getname    = sock_no_getname,
 463         .poll       = datagram_poll,
 464         .ioctl      = sock_no_ioctl,
 465         .listen     = sock_no_listen,
 466         .shutdown   = sock_no_shutdown,
 467         .setsockopt = sock_no_setsockopt,
 468         .getsockopt = sock_no_getsockopt,
 469         .sendmsg    = swift_sendmsg,
 470         .recvmsg    = swift_recvmsg,
 471         .mmap       = sock_no_mmap,
 472         .sendpage   = sock_no_sendpage,
 473 };
 474
 475 static const struct net_protocol swift_protocol = {
 476         .handler   = swift_rcv,
 477         .no_policy = 1,
 478         .netns_ok  = 1,
 479 };
 480
 481 static struct inet_protosw swift_protosw = {
 482         .type     = SOCK_DGRAM,
 483         .protocol = IPPROTO_SWIFT,
 484         .prot     = &swift_prot,
 485         .ops      = &swift_ops,
 486         .no_check = 0,
 487 };
 488
 489 static int __init swift_init(void)
 490 {
 491         int rc;
 492
 493         rc = proto_register(&swift_prot, 1);
 494         if (unlikely(rc)) {
 495                 log_error("Error registering swift protocol\n");
 496                 goto out;
 497         }
 498
 499         rc = inet_add_protocol(&swift_protocol, IPPROTO_SWIFT);
 500         if (unlikely(rc)) {
 501                 log_error("Error adding swift protocol\n");
 502                 goto out_unregister;
 503         }
 504
 505         inet_register_protosw(&swift_protosw);
 506         log_debug("Swift entered\n");
 507
 508         return 0;
 509
 510 out_unregister:
 511         proto_unregister(&swift_prot);
 512
 513 out:
 514         return rc;
 515 }
 516
 517 static void __exit swift_exit(void)
 518 {
 519         inet_unregister_protosw(&swift_protosw);
 520
 521         inet_del_protocol(&swift_protocol, IPPROTO_SWIFT);
 522
 523         proto_unregister(&swift_prot);
 524
 525         log_debug("Swift exited\n");
 526 }
 527
 528 module_init(swift_init);
 529 module_exit(swift_exit);