Index: netinet/tcp_input.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_input.c,v
retrieving revision 1.270
diff -u -p -r1.270 tcp_input.c
--- netinet/tcp_input.c	2 Aug 2007 13:06:30 -0000	1.270
+++ netinet/tcp_input.c	14 Aug 2007 11:33:34 -0000
@@ -981,6 +981,10 @@ tcp_input(struct mbuf *m, ...)
 #endif
 	u_int8_t *optp = NULL;
 	int optlen = 0;
+#ifdef TCP_SIGNATURE
+	u_int8_t *soptp = NULL;
+	int soptlen = 0;
+#endif
 	int len, tlen, toff, hdroptlen = 0;
 	struct tcpcb *tp = 0;
 	int tiflags;
@@ -1142,6 +1146,10 @@ tcp_input(struct mbuf *m, ...)
 		KASSERT(TCP_HDR_ALIGNED_P(th));
 		optlen = off - sizeof (struct tcphdr);
 		optp = ((u_int8_t *)th) + sizeof(struct tcphdr);
+#ifdef TCP_SIGNATURE
+		soptp = optp;
+		soptlen = optlen;
+#endif
 		/*
 		 * Do quick retrieval of timestamp options ("options
 		 * prediction?").  If timestamp is the only option and it's
@@ -1616,13 +1624,17 @@ after_listen:
 	 * Process options.
 	 */
 #ifdef TCP_SIGNATURE
-	if (optp || (tp->t_flags & TF_SIGNATURE))
+	if (optp || (tp->t_flags & TF_SIGNATURE)) {
+		if (!optp) {
+			optp = soptp;
+			optlen = soptlen;
+		}
 #else
-	if (optp)
+	if (optp) {
 #endif
 		if (tcp_dooptions(tp, optp, optlen, th, m, toff, &opti) < 0)
 			goto drop;
-
+	}
 	if (TCP_SACK_ENABLED(tp)) {
 		tcp_del_sackholes(tp, th);
 	}
@@ -2229,7 +2241,12 @@ after_listen:
 	if (tiflags & TH_SYN) {
 		if (tp->rcv_nxt == th->th_seq) {
 			tcp_respond(tp, m, m, th, (tcp_seq)0, th->th_ack - 1,
-			    TH_ACK);
+			    TH_ACK,
+#ifdef TCP_SIGNATURE
+				tp->t_flags & TF_SIGNATURE);
+#else
+				0);
+#endif
 			if (tcp_saveti)
 				m_freem(tcp_saveti);
 			return;
@@ -2759,13 +2776,26 @@ dropwithreset:
 			goto drop;
 	}
 
-	if (tiflags & TH_ACK)
-		(void)tcp_respond(tp, m, m, th, (tcp_seq)0, th->th_ack, TH_RST);
-	else {
+	if (tiflags & TH_ACK) {
+#ifdef TCP_SIGNATURE
+		if(tcp_checksignature(soptp, soptlen))
+		    (void)tcp_respond(tp, m, m, th, (tcp_seq)0, th->th_ack,
+			TH_RST, TF_SIGNATURE);
+		else
+#endif
+		    (void)tcp_respond(tp, m, m, th, (tcp_seq)0, th->th_ack,
+			TH_RST, 0);
+	} else {
 		if (tiflags & TH_SYN)
 			tlen++;
-		(void)tcp_respond(tp, m, m, th, th->th_seq + tlen, (tcp_seq)0,
-		    TH_RST|TH_ACK);
+#ifdef TCP_SIGNATURE
+		if(tcp_checksignature(soptp, soptlen))
+		    (void)tcp_respond(tp, m, m, th, th->th_seq + tlen,
+			(tcp_seq)0, TH_RST|TH_ACK, TF_SIGNATURE);
+		else
+#endif
+		    (void)tcp_respond(tp, m, m, th, th->th_seq + tlen, (tcp_seq)0,
+			TH_RST|TH_ACK, 0);
 	}
 	if (tcp_saveti)
 		m_freem(tcp_saveti);
@@ -2806,7 +2836,7 @@ tcp_signature_apply(void *fstate, void *
 }
 
 struct secasvar *
-tcp_signature_getsav(struct mbuf *m, struct tcphdr *th)
+tcp_signature_getsav(struct mbuf *m)
 {
 	struct secasvar *sav;
 #ifdef FAST_IPSEC
@@ -2855,7 +2885,7 @@ tcp_signature_getsav(struct mbuf *m, str
 		sav = key_allocsa(AF_INET6, (void *)&ip6->ip6_src,
 		    (void *)&ip6->ip6_dst, IPPROTO_TCP,
 		    htonl(TCP_SIG_SPI), 0, 0);
-#endif
+#endif	/* FAST_IPSEC */
 
 	return (sav);	/* freesav must be performed by caller */
 }
@@ -2928,7 +2958,34 @@ tcp_signature(struct mbuf *m, struct tcp
 
 	return (0);
 }
-#endif
+
+/*
+ * Checks if signature option is present
+ */
+int
+tcp_checksignature(const u_char *cp, int cnt)
+{
+	int i = 0;
+
+	if (cp == NULL)
+		return 0;
+
+	while (i < cnt) {
+		if (cp[i] == TCPOPT_EOL)
+			return 0;
+		if (cp[i] == TCPOPT_NOP) {
+			i++;
+			continue;
+		}
+		if (cp[i] == TCPOPT_SIGNATURE)
+			return 1;
+		i += cp[i+1];
+	}
+
+return 0;
+}
+
+#endif	/* TCP_SIGNATURE */
 
 static int
 tcp_dooptions(struct tcpcb *tp, const u_char *cp, int cnt,
@@ -3049,7 +3106,7 @@ tcp_dooptions(struct tcpcb *tp, const u_
 		case TCPOPT_SIGNATURE:
 			if (optlen != TCPOLEN_SIGNATURE)
 				continue;
-			if (sigp && bcmp(sigp, cp + 2, TCP_SIGLEN))
+			if (sigp && memcmp(sigp, cp + 2, TCP_SIGLEN))
 				return (-1);
 
 			sigp = sigbuf;
@@ -3063,7 +3120,7 @@ tcp_dooptions(struct tcpcb *tp, const u_
 #ifdef TCP_SIGNATURE
 	if (tp->t_flags & TF_SIGNATURE) {
 
-		sav = tcp_signature_getsav(m, th);
+		sav = tcp_signature_getsav(m);
 
 		if (sav == NULL && tp->t_state == TCPS_LISTEN)
 			return (-1);
@@ -3097,7 +3154,7 @@ tcp_dooptions(struct tcpcb *tp, const u_
 		}
 		TCP_FIELDS_TO_HOST(th);
 
-		if (bcmp(sig, sigp, TCP_SIGLEN)) {
+		if (memcmp(sig, sigp, TCP_SIGLEN)) {
 			tcpstat.tcps_badsig++;
 			if (sav == NULL)
 				return (-1);
@@ -3521,8 +3578,8 @@ syn_cache_lookup(const struct sockaddr *
 	     sc = TAILQ_NEXT(sc, sc_bucketq)) {
 		if (sc->sc_hash != hash)
 			continue;
-		if (!bcmp(&sc->sc_src, src, src->sa_len) &&
-		    !bcmp(&sc->sc_dst, dst, dst->sa_len)) {
+		if (!memcmp(&sc->sc_src, src, src->sa_len) &&
+		    !memcmp(&sc->sc_dst, dst, dst->sa_len)) {
 			splx(s);
 			return (sc);
 		}
@@ -3849,7 +3906,8 @@ syn_cache_get(struct sockaddr *src, stru
 	return (so);
 
 resetandabort:
-	(void)tcp_respond(NULL, m, m, th, (tcp_seq)0, th->th_ack, TH_RST);
+	/* XXX: TCP_SIGNATURE ? */
+	(void)tcp_respond(NULL, m, m, th, (tcp_seq)0, th->th_ack, TH_RST, 0);
 abort:
 	if (so != NULL)
 		(void) soabort(so);
@@ -4334,7 +4392,7 @@ syn_cache_respond(struct syn_cache *sc, 
 		struct secasvar *sav;
 		u_int8_t *sigp;
 
-		sav = tcp_signature_getsav(m, th);
+		sav = tcp_signature_getsav(m);
 
 		if (sav == NULL) {
 			if (m)
Index: netinet/tcp_output.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_output.c,v
retrieving revision 1.161
diff -u -p -r1.161 tcp_output.c
--- netinet/tcp_output.c	2 Aug 2007 13:12:35 -0000	1.161
+++ netinet/tcp_output.c	14 Aug 2007 11:33:34 -0000
@@ -578,6 +578,7 @@ tcp_output(struct tcpcb *tp)
 	struct sackhole *p;
 #ifdef TCP_SIGNATURE
 	int sigoff = 0;
+	struct secasvar *sav;
 #endif
 
 #ifdef DIAGNOSTIC
@@ -1152,9 +1153,16 @@ send:
 
 				cp[0] = TCPOPT_SACK_PERMITTED;
 				cp[1] = 2;
-				cp[2] = TCPOPT_NOP;
-				cp[3] = TCPOPT_NOP;
-				optlen += 4;
+#ifdef TCP_SIGNATURE
+				if (tp->t_flags & TF_SIGNATURE)
+					optlen += 2;
+				else
+#endif
+				{
+					cp[2] = TCPOPT_NOP;
+					cp[3] = TCPOPT_NOP;
+					optlen += 4;
+				}
 			}
 		}
 	}
@@ -1220,18 +1228,19 @@ send:
 		 * Initialize TCP-MD5 option (RFC2385)
 		 */
 		bp = (u_char *)opt + optlen;
-		*bp++ = TCPOPT_SIGNATURE;
-		*bp++ = TCPOLEN_SIGNATURE;
+		memset(bp, 0, TCPOLEN_SIGNATURE);
+		bp[0] = TCPOPT_SIGNATURE;
+		bp[1] = TCPOLEN_SIGNATURE;
 		sigoff = optlen + 2;
-		bzero(bp, TCP_SIGLEN);
-		bp += TCP_SIGLEN;
-		optlen += TCPOLEN_SIGNATURE;
+
 		/*
-		 * Terminate options list and maintain 32-bit alignment.
+		 * maintain 32-bit alignment as this looks
+		 * to be our last option XXX
  		 */
-		*bp++ = TCPOPT_NOP;
-		*bp++ = TCPOPT_EOL;
- 		optlen += 2;
+		optlen += TCPOLEN_SIGNATURE;
+		for (bp += TCPOLEN_SIGNATURE; optlen % 4; optlen++, bp++)
+			*bp = 0;
+
  	}
 #endif /* TCP_SIGNATURE */
 
@@ -1430,10 +1439,9 @@ send:
 
 #ifdef TCP_SIGNATURE
 	if (sigoff && (tp->t_flags & TF_SIGNATURE)) {
-		struct secasvar *sav;
 		u_int8_t *sigp;
 
-		sav = tcp_signature_getsav(m, th);
+		sav = tcp_signature_getsav(m);
 
 		if (sav == NULL) {
 			if (m)
Index: netinet/tcp_subr.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_subr.c,v
retrieving revision 1.218
diff -u -p -r1.218 tcp_subr.c
--- netinet/tcp_subr.c	2 Aug 2007 02:42:41 -0000	1.218
+++ netinet/tcp_subr.c	14 Aug 2007 11:33:34 -0000
@@ -595,7 +595,7 @@ tcp_template(struct tcpcb *tp)
  */
 int
 tcp_respond(struct tcpcb *tp, struct mbuf *template, struct mbuf *m,
-    struct tcphdr *th0, tcp_seq ack, tcp_seq seq, int flags)
+    struct tcphdr *th0, tcp_seq ack, tcp_seq seq, int flags, int opts)
 {
 	struct route *ro;
 	int error, tlen, win = 0;
@@ -607,6 +607,9 @@ tcp_respond(struct tcpcb *tp, struct mbu
 	int family;	/* family on packet, not inpcb/in6pcb! */
 	struct tcphdr *th;
 	struct socket *so;
+#ifdef TCP_SIGNATURE
+	struct secasvar *sav;
+#endif
 
 	if (tp != NULL && (flags & TH_RST) == 0) {
 #ifdef DIAGNOSTIC
@@ -726,12 +729,25 @@ tcp_respond(struct tcpcb *tp, struct mbu
 			tlen = sizeof(*th0);
 		else
 			tlen = th0->th_off << 2;
-
+#ifdef TCP_SIGNATURE
+		if (opts & TF_SIGNATURE)
+			tlen += TCPOLEN_SIGLEN;
+#endif
 		if (m->m_len > hlen + tlen && (m->m_flags & M_EXT) == 0 &&
 		    mtod(m, char *) + hlen == (char *)th0) {
 			m->m_len = hlen + tlen;
 			m_freem(m->m_next);
 			m->m_next = NULL;
+#ifdef TCP_SIGNATURE
+		    if (opts & TF_SIGNATURE) {
+			char *sigplace;
+			sigplace = mtod(m, char*);
+			sigplace += hlen + tlen - TCPOLEN_SIGLEN;
+			memset(sigplace, 0, TCPOLEN_SIGLEN);
+			sigplace[0] = TCPOPT_SIGNATURE;
+			sigplace[1] = TCPOLEN_SIGNATURE;
+		    }
+#endif
 		} else {
 			struct mbuf *n;
 
@@ -758,8 +774,20 @@ tcp_respond(struct tcpcb *tp, struct mbu
 			n->m_data += max_linkhdr;
 			n->m_len = hlen + tlen;
 			m_copyback(n, 0, hlen, mtod(m, void *));
+#ifndef TCP_SIGNATURE
 			m_copyback(n, hlen, tlen, (void *)th0);
-
+#else
+			if (opts & TF_SIGNATURE) {
+				char sigplace[TCPOLEN_SIGLEN];
+				m_copyback(n, hlen, tlen - TCPOLEN_SIGLEN, (void *)th0);
+				memset(sigplace, 0, TCPOLEN_SIGLEN);
+				sigplace[0] = TCPOPT_SIGNATURE;
+				sigplace[1] = TCPOLEN_SIGNATURE;
+				m_copyback(n, hlen + tlen - TCPOLEN_SIGLEN, TCPOLEN_SIGLEN,
+					(void*)sigplace);
+			} else
+			    m_copyback(n, hlen, tlen, (void *)th0);
+#endif
 			m_freem(m);
 			m = n;
 			n = NULL;
@@ -807,12 +835,33 @@ tcp_respond(struct tcpcb *tp, struct mbu
 		tlen += sizeof(*th);
 	} else
 		tlen += th->th_off << 2;
+#ifdef TCP_SIGNATURE
+	/* again and again */
+	if (opts & TF_SIGNATURE) {
+		tlen += TCPOLEN_SIGLEN;
+		th->th_off += TCPOLEN_SIGLEN >> 2;
+	}
+#endif
 	m->m_len = hlen + tlen;
 	m->m_pkthdr.len = hlen + tlen;
 	m->m_pkthdr.rcvif = (struct ifnet *) 0;
 	th->th_flags = flags;
 	th->th_urp = 0;
-
+#ifdef TCP_SIGNATURE
+	sav = tcp_signature_getsav(m);
+	if (sav) {
+		tcp_signature(m, th, (char *)th - mtod(m, char *),
+			sav, (char*)th + (th->th_off << 2) - TCPOLEN_SIGNATURE);
+		key_sa_recordxfer(sav, m);
+#ifdef FAST_IPSEC
+		KEY_FREESAV(&sav);
+#else
+		key_freesav(sav);
+#endif
+	} else
+		memset((char*)th + (th->th_off << 2) - TCPOLEN_SIGNATURE, 0,
+			TCPOLEN_SIGNATURE);
+#endif	/* TCP_SIGNATURE */
 	switch (family) {
 #ifdef INET
 	case AF_INET:
Index: netinet/tcp_timer.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_timer.c,v
retrieving revision 1.77
diff -u -p -r1.77 tcp_timer.c
--- netinet/tcp_timer.c	20 Jun 2007 15:29:18 -0000	1.77
+++ netinet/tcp_timer.c	14 Aug 2007 11:33:34 -0000
@@ -569,11 +569,21 @@ tcp_timer_keep(void *arg)
 			 */
 			(void)tcp_respond(tp, tp->t_template,
 			    (struct mbuf *)NULL, NULL, tp->rcv_nxt - 1,
-			    tp->snd_una - 1, 0);
+			    tp->snd_una - 1, 0,
+#ifdef TCP_SIGNATURE
+			    tp->t_flags & TF_SIGNATURE);
+#else
+			    0);
+#endif
 		} else {
 			(void)tcp_respond(tp, tp->t_template,
 			    (struct mbuf *)NULL, NULL, tp->rcv_nxt,
-			    tp->snd_una - 1, 0);
+			    tp->snd_una - 1, 0,
+#ifdef TCP_SIGNATURE
+			    tp->t_flags & TF_SIGNATURE);
+#else
+			    0);
+#endif
 		}
 		TCP_TIMER_ARM(tp, TCPT_KEEP, tp->t_keepintvl);
 	} else
Index: netinet/tcp_usrreq.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_usrreq.c,v
retrieving revision 1.136
diff -u -p -r1.136 tcp_usrreq.c
--- netinet/tcp_usrreq.c	2 Aug 2007 02:42:41 -0000	1.136
+++ netinet/tcp_usrreq.c	14 Aug 2007 11:33:34 -0000
@@ -722,10 +722,10 @@ tcp_ctloutput(int op, struct socket *so,
 
 #ifdef TCP_SIGNATURE
 		case TCP_MD5SIG:
-			if (m == NULL || m->m_len < sizeof (int))
+			if (m == NULL || m->m_len < sizeof (int)) {
 				error = EINVAL;
-			if (error)
 				break;
+			}
 			if (*mtod(m, int *) > 0)
 				tp->t_flags |= TF_SIGNATURE;
 			else
Index: netinet/tcp_var.h
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_var.h,v
retrieving revision 1.150
diff -u -p -r1.150 tcp_var.h
--- netinet/tcp_var.h	2 Aug 2007 02:42:42 -0000	1.150
+++ netinet/tcp_var.h	14 Aug 2007 11:33:34 -0000
@@ -851,9 +851,10 @@ struct tcpcb *
 	 tcp_drop(struct tcpcb *, int);
 #ifdef TCP_SIGNATURE
 int	 tcp_signature_apply(void *, void *, u_int);
-struct secasvar *tcp_signature_getsav(struct mbuf *, struct tcphdr *);
+struct secasvar *tcp_signature_getsav(struct mbuf *);
 int	 tcp_signature(struct mbuf *, struct tcphdr *, int, struct secasvar *,
 	    char *);
+int	tcp_checksignature(const u_char *, int);
 #endif
 void	 tcp_drain(void);
 void	 tcp_established(struct tcpcb *);
@@ -886,7 +887,7 @@ struct ipqent *tcpipqent_alloc(void);
 void	 tcpipqent_free(struct ipqent *);
 
 int	 tcp_respond(struct tcpcb *, struct mbuf *, struct mbuf *,
-	    struct tcphdr *, tcp_seq, tcp_seq, int);
+	    struct tcphdr *, tcp_seq, tcp_seq, int, int);
 void	 tcp_rmx_rtt(struct tcpcb *);
 void	 tcp_setpersist(struct tcpcb *);
 #ifdef TCP_SIGNATURE
Index: netkey/key.c
===================================================================
RCS file: /cvsroot/src/sys/netkey/key.c,v
retrieving revision 1.156
diff -u -p -r1.156 key.c
--- netkey/key.c	9 Jul 2007 21:11:14 -0000	1.156
+++ netkey/key.c	14 Aug 2007 11:33:35 -0000
@@ -8232,7 +8232,8 @@ key_sa_routechange(dst)
 
 	LIST_FOREACH(sah, &sahtree, chain) {
 		ro = &sah->sa_route;
-		if (dst->sa_len == rtcache_getdst(ro)->sa_len &&
+		if (sah->saidx.proto != IPPROTO_TCP &&
+		    dst->sa_len == rtcache_getdst(ro)->sa_len &&
 		    memcmp(dst, rtcache_getdst(ro), dst->sa_len) == 0)
 			rtcache_free(ro);
 	}