1 #include <linux/module.h>
3 #include <net/protocol.h>
10 MODULE_DESCRIPTION("Swift Transport Protocol");
11 MODULE_AUTHOR("Adrian Bondrescu/Cornel Mercan");
12 MODULE_LICENSE("GPL");
15 struct inet_sock sock;
16 /* swift socket speciffic data */
21 static struct swift_sock * sock_port_map[MAX_SWIFT_PORT];
23 static inline struct swift_sock * swift_sk(struct sock * sock)
25 return (struct swift_sock *)(sock);
28 static inline struct swifthdr * swift_hdr(const struct sk_buff * skb)
30 return (struct swifthdr *) skb_transport_header(skb);
33 static inline uint8_t get_next_free_port(void)
36 for (i = MIN_SWIFT_PORT; i < MAX_SWIFT_PORT; i ++)
37 if (sock_port_map[i] == NULL)
42 static inline void swift_unhash(uint8_t port)
44 sock_port_map[port] = NULL;
47 static inline void swift_hash(uint8_t port, struct swift_sock *ssh)
49 sock_port_map[port] = ssh;
52 static inline struct swift_sock * swift_lookup(uint8_t port)
54 return sock_port_map[port];
57 static int swift_release(struct socket *sock)
59 struct sock *sk = sock->sk;
60 struct swift_sock * ssk = swift_sk(sk);
65 swift_unhash(ssk->src);
67 sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
74 skb_queue_purge(&sk->sk_receive_queue);
76 log_debug("swift_release sock=%p\n", sk);
82 static int swift_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
84 struct sockaddr_swift *swift_addr;
85 struct swift_sock *ssk;
89 if (unlikely(addr_len < sizeof(struct sockaddr_swift))) {
90 log_error("Invalid size for sockaddr\n");
95 swift_addr = (struct sockaddr_swift *) addr;
97 if (unlikely(swift_addr->sin_family != AF_INET)) {
98 log_error("Invalid family for sockaddr\n");
103 port = ntohs(swift_addr->sin_port);
105 if (unlikely(port == 0 || port >= MAX_SWIFT_PORT)) {
106 log_error("Invalid value for sockaddr port (%u)\n", port);
111 if (unlikely(swift_lookup(port) != NULL)) {
112 log_error("Port %u already in use\n", port);
117 ssk = swift_sk(sock->sk);
120 swift_hash(port, ssk);
122 log_debug("Socket %p bound to port %u\n", ssk, port);
130 static int swift_connect(struct socket *sock, struct sockaddr *addr, int addr_len, int flags)
134 struct inet_sock * isk;
135 struct swift_sock * ssk;
137 log_debug("swift_connect\n");
139 if (unlikely(sock == NULL)) {
140 log_error("Sock is NULL\n");
146 if (unlikely(sk == NULL)) {
147 log_error("Sock->sk is NULL\n");
155 if (unlikely(ssk->src != 0)) {
156 log_error("ssk->src is not NULL\n");
162 struct sockaddr_swift * swift_addr = (struct sockaddr_swift *) addr;
164 if (unlikely(addr_len < sizeof(*swift_addr) || swift_addr->sin_family != AF_INET)) {
165 log_error("Invalid size or address family\n");
169 ssk->dst = ntohs(swift_addr->sin_port);
170 if (unlikely(ssk->dst == 0 || ssk->dst >= MAX_SWIFT_PORT)) {
171 log_error("Invalid value for destination port(%u)\n", ssk->dst);
176 isk->inet_daddr = swift_addr->sin_addr.s_addr;
177 log_debug("Received from user space destination port=%u and address=%u\n", ssk->dst, isk->inet_daddr);
179 log_error("Invalid swift_addr (NULL)\n");
184 ssk->src = get_next_free_port();
185 if (unlikely(ssk->src == 0)) {
186 log_error("No free ports\n");
191 swift_hash(ssk->src, ssk);
199 static int swift_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
205 struct sk_buff * skb;
207 struct inet_sock * isk;
208 struct swift_sock * ssk;
209 struct swifthdr * shdr;
212 struct rtable * rt = NULL;
214 if (unlikely(sock == NULL)) {
215 log_error("Sock is NULL\n");
221 if (unlikely(sk == NULL)) {
222 log_error("Sock->sk is NULL\n");
232 sport = get_next_free_port();
233 if (unlikely(sport == 0)) {
234 log_error("No free ports\n");
241 struct sockaddr_swift * swift_addr = (struct sockaddr_swift *) msg->msg_name;
243 if (unlikely(msg->msg_namelen < sizeof(*swift_addr) || swift_addr->sin_family != AF_INET)) {
244 log_error("Invalid size or address family\n");
249 dport = ntohs(swift_addr->sin_port);
250 if (unlikely(dport == 0 || dport >= MAX_SWIFT_PORT)) {
251 log_error("Invalid value for destination port(%u)\n", dport);
256 daddr = swift_addr->sin_addr.s_addr;
257 log_debug("Received from user space destination port=%u and address=%u\n", dport, daddr);
259 if (unlikely(!ssk->dst || !isk->inet_daddr)) {
260 log_error("No destination port/address\n");
265 daddr = isk->inet_daddr;
267 log_debug("Got from socket destination port=%u and address=%u\n", dport, daddr);
271 totlen = len + sizeof(struct swifthdr) + sizeof(struct iphdr);
272 skb = sock_alloc_send_skb(sk, totlen, msg->msg_flags & MSG_DONTWAIT, &err);
273 if (unlikely(!skb)) {
274 log_error("sock_alloc_send_skb failed\n");
277 log_debug("Allocated %u bytes for skb (payload size=%u)\n", totlen, len);
279 skb_reset_network_header(skb);
280 skb_reserve(skb, sizeof(struct iphdr));
281 log_debug("Reseted network header\n");
282 skb_reset_transport_header(skb);
283 skb_put(skb, sizeof(struct swifthdr));
284 log_debug("Reseted transport header\n");
286 shdr = (struct swifthdr *) skb_transport_header(skb);
287 shdr->dst = ntohs(dport);
288 shdr->src = ntohs(sport);
289 shdr->len = ntohs(len + sizeof(struct swifthdr));
291 log_debug("payload=%p\n", skb_put(skb, len));
293 err = skb_copy_datagram_from_iovec(skb, sizeof(struct swifthdr), msg->msg_iov, 0, len);
295 log_error("skb_copy_datagram_from_iovec failed\n");
298 log_debug("Copied %u bytes into the skb\n", len);
301 rt = (struct rtable *) __sk_dst_check(sk, 0);
304 struct flowi fl = { .fl4_dst = daddr,
305 .proto = sk->sk_protocol,
306 .flags = inet_sk_flowi_flags(sk),
308 err = ip_route_output_flow(sock_net(sk), &rt, &fl, sk, 0);
310 log_error("Route lookup failed\n");
313 sk_dst_set(sk, dst_clone(&rt->dst));
316 err = ip_queue_xmit(skb);
318 log_debug("Sent %u bytes on wire\n", len);
320 log_error("ip_queue_xmit failed\n");
331 static int swift_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
334 struct sockaddr_swift *swift_addr;
335 struct sock * sk = sock->sk;
338 skb = skb_recv_datagram(sk, flags, flags & MSG_DONTWAIT, &err);
339 if (unlikely(!skb)) {
340 log_error("skb_recv_datagram\n");
344 log_debug("Received skb %p\n", skb);
346 swift_addr = (struct sockaddr_swift *) skb->cb;
347 msg->msg_namelen = sizeof(struct sockaddr_swift);
352 msg->msg_flags |= MSG_TRUNC;
355 err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
357 log_error("skb_copy_datagram_iovec\n");
361 sock_recv_ts_and_drops(msg, sk, skb);
364 memcpy(msg->msg_name, swift_addr, msg->msg_namelen);
369 skb_free_datagram(sk, skb);
375 static int swift_rcv(struct sk_buff *skb)
377 struct swifthdr *shdr;
378 struct swift_sock *ssk;
381 struct sockaddr_swift * swift_addr;
384 if (unlikely(!pskb_may_pull(skb, sizeof(struct swifthdr)))) {
385 log_error("Insufficient space for header\n");
389 shdr = (struct swifthdr *) skb->data;
390 len = ntohs(shdr->len);
392 if (unlikely(skb->len < len)) {
393 log_error("Malformed packet (packet_len=%u, skb_len=%u)\n", len, skb->len);
397 if (unlikely(len < sizeof(struct swifthdr))) {
398 log_error("Malformed packet (packet_len=%u sizeof(swifthdr)=%u\n", len, sizeof(struct swifthdr));
402 src = ntohs(shdr->src);
403 dst = ntohs(shdr->dst);
404 if (unlikely(src == 0 || dst == 0 || src >= MAX_SWIFT_PORT || dst >= MAX_SWIFT_PORT)) {
405 log_error("Malformed packet (src=%u, dst=%u)\n", shdr->src, shdr->dst);
409 skb_pull(skb, sizeof(struct swifthdr));
410 len -= sizeof(struct swifthdr);
414 log_debug("Received %u bytes from from port=%u to port=%u\n", len - sizeof(struct swifthdr), src, dst);
416 ssk = swift_lookup(dst);
418 log_error("Swift lookup failed for port %u\n", dst);
422 BUILD_BUG_ON(sizeof(struct sockaddr_swift) > sizeof(skb->cb));
424 swift_addr = (struct sockaddr_swift *) skb->cb;
425 swift_addr->sin_family = AF_INET;
426 swift_addr->sin_port = shdr->src;
427 swift_addr->sin_addr.s_addr = ip_hdr(skb)->saddr;
429 log_debug("Setting sin_port=%u, sin_addr=%u\n", ntohs(shdr->src), swift_addr->sin_addr.s_addr);
431 err = ip_queue_rcv_skb((struct sock *) &ssk->sock, skb);
433 log_error("ip_queu_rcv_skb\n");
436 return NET_RX_SUCCESS;
443 static struct proto swift_prot = {
444 .obj_size = sizeof(struct swift_sock),
445 .owner = THIS_MODULE,
449 static const struct proto_ops swift_ops = {
451 .owner = THIS_MODULE,
452 .release = swift_release,
454 .connect = swift_connect,
455 .socketpair = sock_no_socketpair,
456 .accept = sock_no_accept,
457 .getname = sock_no_getname,
458 .poll = datagram_poll,
459 .ioctl = sock_no_ioctl,
460 .listen = sock_no_listen,
461 .shutdown = sock_no_shutdown,
462 .setsockopt = sock_no_setsockopt,
463 .getsockopt = sock_no_getsockopt,
464 .sendmsg = swift_sendmsg,
465 .recvmsg = swift_recvmsg,
466 .mmap = sock_no_mmap,
467 .sendpage = sock_no_sendpage,
470 static const struct net_protocol swift_protocol = {
471 .handler = swift_rcv,
476 static struct inet_protosw swift_protosw = {
478 .protocol = IPPROTO_SWIFT,
484 static int __init swift_init(void)
488 rc = proto_register(&swift_prot, 1);
490 log_error("Error registering swift protocol\n");
494 rc = inet_add_protocol(&swift_protocol, IPPROTO_SWIFT);
496 log_error("Error adding swift protocol\n");
500 inet_register_protosw(&swift_protosw);
501 log_debug("Swift entered\n");
506 proto_unregister(&swift_prot);
512 static void __exit swift_exit(void)
514 inet_unregister_protosw(&swift_protosw);
516 inet_del_protocol(&swift_protocol, IPPROTO_SWIFT);
518 proto_unregister(&swift_prot);
520 log_debug("Swift exited\n");
523 module_init(swift_init);
524 module_exit(swift_exit);