From 08dba0109c585f329b17849571db1676d14e8bb2 Mon Sep 17 00:00:00 2001
From: Andy <andy@iwalk.me>
Date: Mon, 4 Sep 2017 04:26:11 -0400
Subject: [PATCH] add lvs toa option

---
 freebsd/netinet/tcp_syncache.c | 72 ++++++++++++++++++++++++++++++++++
 freebsd/netinet/tcp_usrreq.c   | 52 ++++++++++++++++++++++++
 freebsd/sys/socketvar.h        |  3 ++
 lib/Makefile                   |  3 ++
 4 files changed, 130 insertions(+)

diff --git a/freebsd/netinet/tcp_syncache.c b/freebsd/netinet/tcp_syncache.c
index 5bb18b0d..a967eba3 100644
--- a/freebsd/netinet/tcp_syncache.c
+++ b/freebsd/netinet/tcp_syncache.c
@@ -629,6 +629,73 @@ done:
 	SCH_UNLOCK(sch);
 }
 
+#ifdef LVS_TCPOPT_TOA
+
+#ifndef TCPOPT_TOA
+#define TCPOPT_TOA 254
+#define TCPOLEN_TOA  8
+#endif
+
+static void
+syncache_get_toa(struct mbuf *m, struct socket *so)
+{
+    int length;
+    u_char *ptr = NULL;
+    struct tcphdr *th = NULL;
+
+    struct ipovly *ipov = mtod(m, struct ipovly *);
+    struct ip *ip = (struct ip *)ipov;
+
+#ifdef INET6
+    struct ip6_hdr *ip6 = (struct ip6_hdr *)ipov;
+
+    if (ip->ip_v == 6) {
+        /*
+         * FIXME: ipv6 not support now
+         * th = ???
+         */
+
+        return;
+
+    } else
+#endif
+    {
+        th = (struct tcphdr *)((caddr_t)ip + (ntohs(ip->ip_len) - ntohs(ipov->ih_len)));
+    }
+
+    length = (th->th_off << 2) - sizeof (struct tcphdr);
+    ptr = (u_char *)(th + 1);
+
+    while (length > 0) {
+        int opcode = *ptr++;
+        int opsize;
+
+        switch (opcode) {
+        case TCPOPT_EOL:
+            return;
+
+        case TCPOPT_NOP:
+            length--;
+            continue;
+
+        default:
+            opsize = *ptr++;
+            if (opsize < 2 || opsize > length) {
+                return;
+            }
+
+            if (TCPOPT_TOA == opcode && TCPOLEN_TOA == opsize) {
+                bcopy(ptr - 2, so->so_toa, TCPOLEN_TOA);
+                return;
+            }
+
+            ptr += opsize - 2;
+            length -= opsize;
+        }
+    }
+}
+#endif
+
 /*
  * Build a new TCP socket structure from a syncache entry.
  *
@@ -668,6 +735,11 @@ syncache_socket(struct syncache *sc, struct socket *lso, struct mbuf *m)
 		}
 		goto abort2;
 	}
+
+#ifdef LVS_TCPOPT_TOA
+	syncache_get_toa(m, so);
+#endif
+
 #ifdef MAC
 	mac_socketpeer_set_from_mbuf(m, so);
 #endif
diff --git a/freebsd/netinet/tcp_usrreq.c b/freebsd/netinet/tcp_usrreq.c
index 054dd93b..b33efedc 100644
--- a/freebsd/netinet/tcp_usrreq.c
+++ b/freebsd/netinet/tcp_usrreq.c
@@ -267,6 +267,44 @@ tcp_usr_detach(struct socket *so)
 		INP_INFO_RUNLOCK(&V_tcbinfo);
 }
 
+#ifdef LVS_TCPOPT_TOA
+
+#ifndef TCPOPT_TOA
+#define TCPOPT_TOA 254
+#define TCPOLEN_TOA  8
+#endif
+
+struct toa_data { 
+    uint8_t opcode;
+    uint8_t opsize;
+    uint16_t port;
+    uint32_t ip;
+};
+
+static int
+toa_getpeeraddr(struct socket *so, struct sockaddr **nam)
+{
+    int ret;
+    struct toa_data *toa;
+    struct sockaddr_in *sin;
+
+    ret = in_getpeeraddr(so, nam);
+    if (ret) {
+        return ret;
+    }
+
+    toa = (struct toa_data *)so->so_toa;
+    if (toa->opcode == TCPOPT_TOA && toa->opsize == TCPOLEN_TOA) {
+        sin = (struct sockaddr_in *)(*nam);
+
+        sin->sin_addr.s_addr = toa->ip;
+        sin->sin_port = toa->port;
+    }
+
+    return 0;
+}
+#endif
+
 #ifdef INET
 /*
  * Give the socket an address.
@@ -697,6 +735,16 @@ tcp_usr_accept(struct socket *so, struct sockaddr **nam)
 	port = inp->inp_fport;
 	addr = inp->inp_faddr;
 
+#ifdef LVS_TCPOPT_TOA
+{
+	struct toa_data *toa = (struct toa_data *)so->so_toa;
+	if (toa->opcode == TCPOPT_TOA && toa->opsize == TCPOLEN_TOA) {
+		addr.s_addr = toa->ip;
+		port = toa->port;
+	}
+}
+#endif
+
 out:
 	TCPDEBUG2(PRU_ACCEPT);
 	TCP_PROBE2(debug__user, tp, PRU_ACCEPT);
@@ -1160,7 +1208,11 @@ struct pr_usrreqs tcp_usrreqs = {
 	.pru_detach =		tcp_usr_detach,
 	.pru_disconnect =	tcp_usr_disconnect,
 	.pru_listen =		tcp_usr_listen,
+#ifdef LVS_TCPOPT_TOA
+	.pru_peeraddr =		toa_getpeeraddr,
+#else
 	.pru_peeraddr =		in_getpeeraddr,
+#endif
 	.pru_rcvd =		tcp_usr_rcvd,
 	.pru_rcvoob =		tcp_usr_rcvoob,
 	.pru_send =		tcp_usr_send,
diff --git a/freebsd/sys/socketvar.h b/freebsd/sys/socketvar.h
index 3bc2bae2..39147b29 100644
--- a/freebsd/sys/socketvar.h
+++ b/freebsd/sys/socketvar.h
@@ -129,6 +129,9 @@ struct socket {
 
 	void *so_pspare[2];	/* packet pacing / general use */
 	int so_ispare[2];	/* packet pacing / general use */
+#ifdef LVS_TCPOPT_TOA
+	uint8_t so_toa[8];	/* lvs toa option */
+#endif
 };
 
 /*
diff --git a/lib/Makefile b/lib/Makefile
index ba4aaf05..50d269dc 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -94,6 +94,9 @@ endif
 
 CFLAGS+= -DFSTACK
 
+# add for LVS tcp option toa, disabled by default
+# CFLAGS+= -DLVS_TCPOPT_TOA
+
 VPATH+= $S/${MACHINE_CPUARCH}/${MACHINE_CPUARCH}
 VPATH+= $S/kern
 VPATH+= $S/crypto