+ mptp_hash(ssk->src, ssk);
+
+ return 0;
+
+out:
+ return err;
+}
+
+static int mptp_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
+{
+ int err;
+ uint8_t dport;
+ __be32 daddr;
+ uint8_t sport;
+ struct sk_buff * skb;
+ struct sock * sk;
+ struct inet_sock * isk;
+ struct mptp_sock * ssk;
+ struct mptphdr * shdr;
+ int connected = 0;
+ int totlen;
+ struct rtable * rt = NULL;
+ int dests = 0;
+ int i;
+ struct sockaddr_mptp * mptp_addr = NULL;
+
+ if (unlikely(sock == NULL)) {
+ log_error("Sock is NULL\n");
+ err = -EINVAL;
+ goto out;
+ }
+ sk = sock->sk;
+
+ if (unlikely(sk == NULL)) {
+ log_error("Sock->sk is NULL\n");
+ err = -EINVAL;
+ goto out;
+ }
+
+ isk = inet_sk(sk);
+ ssk = mptp_sk(sk);
+
+ sport = ssk->src;
+ if (sport == 0) {
+ sport = get_next_free_port();
+ if (unlikely(sport == 0)) {
+ log_error("No free ports\n");
+ err = -ENOMEM;
+ goto out;
+ }
+ }
+
+ if (msg->msg_name) {
+ mptp_addr = (struct sockaddr_mptp *) msg->msg_name;
+
+ if (unlikely(msg->msg_namelen < sizeof(*mptp_addr) ||
+ msg->msg_namelen < mptp_addr->count * sizeof(struct mptp_dest) ||
+ mptp_addr->count <= 0)) {
+ log_error("Invalid size for msg_name\n");
+ err = -EINVAL;
+ goto out;
+ }
+
+ dests = mptp_addr->count;
+ } else {
+ BUG();
+ if (unlikely(!ssk->dst || !isk->inet_daddr)) {
+ log_error("No destination port/address\n");
+ err = -EDESTADDRREQ;
+ goto out;
+ }
+ dport = ssk->dst;
+ daddr = isk->inet_daddr;
+
+ log_debug("Got from socket destination port=%u and address=%u\n", dport, daddr);
+ connected = 1;
+ }
+
+ if (msg->msg_iovlen < dests)
+ dests = msg->msg_iovlen;
+
+ for (i = 0; i < dests; i++) {
+ struct mptp_dest *dest = &mptp_addr->dests[i];
+ struct iovec *iov = &msg->msg_iov[i];
+ char *payload;
+
+ dport = dest->port;
+ if (unlikely(dport == 0 || dport >= MAX_MPTP_PORT)) {
+ log_error("Invalid value for destination port(%u)\n", dport);
+ err = -EINVAL;
+ goto out;
+ }
+
+ daddr = dest->addr;
+ log_debug("Received from user space destination port=%u and address=%u\n", dport, daddr);
+
+ len = iov->iov_len;
+ totlen = len + sizeof(struct mptphdr) + sizeof(struct iphdr);
+ skb = sock_alloc_send_skb(sk, totlen, msg->msg_flags & MSG_DONTWAIT, &err);
+ if (unlikely(!skb)) {
+ log_error("sock_alloc_send_skb failed\n");
+ goto out;
+ }
+ log_debug("Allocated %u bytes for skb (payload size=%u)\n", totlen, len);
+
+ skb_reset_network_header(skb);
+ skb_reserve(skb, sizeof(struct iphdr));
+ log_debug("Reseted network header\n");
+ skb_reset_transport_header(skb);
+ skb_put(skb, sizeof(struct mptphdr));
+ log_debug("Reseted transport header\n");
+
+ shdr = (struct mptphdr *) skb_transport_header(skb);
+ shdr->dst = dport;
+ shdr->src = sport;
+ shdr->len = ntohs(len + sizeof(struct mptphdr));
+
+ payload = skb_put(skb, len);
+ log_debug("payload=%p\n", payload);
+
+ err = skb_copy_datagram_from_iovec(skb, sizeof(struct mptphdr), iov, 0, len);
+ if (unlikely(err)) {
+ log_error("skb_copy_datagram_from_iovec failed\n");
+ goto out_free;
+ }
+ log_debug("Copied %u bytes into the skb\n", len);
+
+ if (connected)
+ rt = (struct rtable *) __sk_dst_check(sk, 0);
+
+ if (rt == NULL) {
+ struct flowi fl = { .fl4_dst = daddr,
+ .proto = sk->sk_protocol,
+ .flags = inet_sk_flowi_flags(sk),
+ };
+ err = ip_route_output_flow(sock_net(sk), &rt, &fl, sk, 0);
+ if (unlikely(err)) {
+ log_error("Route lookup failed\n");
+ goto out_free;
+ }
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36)
+ sk_dst_set(sk, dst_clone(&rt->u.dst));
+#else
+ sk_dst_set(sk, dst_clone(&rt->dst));
+#endif
+ }
+
+ skb->local_df = 1;
+ err = ip_queue_xmit(skb);
+ if (likely(!err))
+ log_debug("Sent %u bytes on wire\n", len);
+ else
+ log_error("ip_queue_xmit failed\n");
+ }