Index: tcp_input.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_input.c,v
retrieving revision 1.266
diff -u -r1.266 tcp_input.c
--- tcp_input.c	18 May 2007 21:48:43 -0000	1.266
+++ tcp_input.c	18 May 2007 23:45:03 -0000
@@ -977,6 +977,10 @@
 #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;
@@ -1138,6 +1142,10 @@
 		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
@@ -1612,13 +1620,17 @@
 	 * 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);
 	}
@@ -2158,7 +2170,12 @@
 	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;
@@ -2688,13 +2705,26 @@
 			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);
@@ -2735,7 +2765,7 @@
 }
 
 struct secasvar *
-tcp_signature_getsav(struct mbuf *m, struct tcphdr *th)
+tcp_signature_getsav(struct mbuf *m)
 {
 	struct secasvar *sav;
 #ifdef FAST_IPSEC
@@ -2784,7 +2814,7 @@
 		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 */
 }
@@ -2857,7 +2887,35 @@
 
 	return (0);
 }
-#endif
+
+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)
+		/*
+		 * XXX: I should check the signature
+		 * 	also increment counters...
+		 */
+			return 1;
+		i += cp[i+1];
+	}
+
+return 0;
+}
+
+#endif	/* TCP_SIGNATURE */
 
 static int
 tcp_dooptions(struct tcpcb *tp, const u_char *cp, int cnt,
@@ -2978,7 +3036,7 @@
 		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;
@@ -2992,7 +3050,7 @@
 #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);
@@ -3026,7 +3084,7 @@
 		}
 		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);
@@ -3447,8 +3505,8 @@
 	     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);
 		}
@@ -3775,7 +3833,8 @@
 	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);
@@ -4251,7 +4310,7 @@
 		struct secasvar *sav;
 		u_int8_t *sigp;
 
-		sav = tcp_signature_getsav(m, th);
+		sav = tcp_signature_getsav(m);
 
 		if (sav == NULL) {
 			if (m)
Index: tcp_output.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_output.c,v
retrieving revision 1.159
diff -u -r1.159 tcp_output.c
--- tcp_output.c	18 May 2007 21:48:43 -0000	1.159
+++ tcp_output.c	18 May 2007 23:45:03 -0000
@@ -574,6 +574,7 @@
 	struct sackhole *p;
 #ifdef TCP_SIGNATURE
 	int sigoff = 0;
+	struct secasvar *sav;
 #endif
 
 #ifdef DIAGNOSTIC
@@ -1105,9 +1106,16 @@
 
 				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;
+				}
 			}
 		}
 	}
@@ -1169,18 +1177,19 @@
 		 * 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 */
 
@@ -1379,10 +1388,9 @@
 
 #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: tcp_subr.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_subr.c,v
retrieving revision 1.214
diff -u -r1.214 tcp_subr.c
--- tcp_subr.c	2 May 2007 20:40:25 -0000	1.214
+++ tcp_subr.c	18 May 2007 23:45:03 -0000
@@ -594,7 +594,7 @@
  */
 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;
@@ -606,6 +606,9 @@
 	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
@@ -725,12 +728,25 @@
 			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;
 
@@ -757,8 +773,20 @@
 			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;
@@ -806,12 +834,33 @@
 		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: tcp_timer.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_timer.c,v
retrieving revision 1.76
diff -u -r1.76 tcp_timer.c
--- tcp_timer.c	9 Oct 2006 16:27:07 -0000	1.76
+++ tcp_timer.c	18 May 2007 23:45:03 -0000
@@ -566,11 +566,21 @@
 			 */
 			(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, tcp_keepintvl);
 	} else
Index: tcp_usrreq.c
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_usrreq.c,v
retrieving revision 1.131
diff -u -r1.131 tcp_usrreq.c
--- tcp_usrreq.c	4 Mar 2007 06:03:22 -0000	1.131
+++ tcp_usrreq.c	18 May 2007 23:45:03 -0000
@@ -694,10 +694,10 @@
 
 #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: tcp_var.h
===================================================================
RCS file: /cvsroot/src/sys/netinet/tcp_var.h,v
retrieving revision 1.146
diff -u -r1.146 tcp_var.h
--- tcp_var.h	2 May 2007 20:40:25 -0000	1.146
+++ tcp_var.h	18 May 2007 23:45:03 -0000
@@ -828,9 +828,10 @@
 	 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 *);
@@ -863,7 +864,7 @@
 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