--- ansh/src/utils.c	2011/10/13 11:01:37	1.1.1.1.2.5
+++ ansh/src/utils.c	2011/10/13 14:29:30	1.1.1.1.2.6
@@ -3,7 +3,7 @@
  *  by Michael Pounov <misho@elwix.org>
  *
  * $Author: misho $
- * $Id: utils.c,v 1.1.1.1.2.5 2011/10/13 11:01:37 misho Exp $
+ * $Id: utils.c,v 1.1.1.1.2.6 2011/10/13 14:29:30 misho Exp $
  *
  *************************************************************************/
 #include "global.h"
@@ -121,8 +121,8 @@ PrepareL3(const struct sockaddr *sa, int *bpflen)
 }
 
 char
-icmpRecv(int s, u_short * __restrict id, u_int * __restrict crypted, u_char * __restrict data, 
-		int * __restrict datlen, struct sockaddr *sa, socklen_t *salen)
+icmpRecv(int s, u_int * __restrict seq, u_short * __restrict id, u_int * __restrict crypted, 
+		u_char * __restrict data, int * __restrict datlen, struct sockaddr *sa, socklen_t *salen)
 {
 	int ret = 0;
 	struct icmp *icmp;
@@ -185,13 +185,16 @@ icmpRecv(int s, u_short * __restrict id, u_int * __res
 		memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen);
 	}
 
+	if (seq)
+		*seq = ntohl(hdr->ansh_seq);
 	if (id)
 		*id = ntohs(icmp->icmp_id);
 	return hdr->ansh_flg;
 }
 
 int
-icmpSend(int s, u_short id, char flg, u_int crypted, u_char *data, int datlen, struct sockaddr *sa, socklen_t salen)
+icmpSend(int s, u_int seq, u_short id, char flg, u_int crypted, u_char *data, int datlen, 
+		struct sockaddr *sa, socklen_t salen)
 {
 	u_char *pos, buf[USHRT_MAX] = { 0 };
 	struct icmp *icmp;
@@ -212,6 +215,7 @@ icmpSend(int s, u_short id, char flg, u_int crypted, u
 	hdr->ansh_flg = flg;
 	hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
 	hdr->ansh_nonce = htonl(crypted);
+	hdr->ansh_seq = htonl(seq);
 	hdr->ansh_crc = 0;
 	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
 
@@ -238,7 +242,7 @@ icmpSend(int s, u_short id, char flg, u_int crypted, u
 }
 
 static int
-_pkt_Send(int s, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea)
+_pkt_Send(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea)
 {
 	u_char *pos, buf[USHRT_MAX] = { 0 };
 	struct ether_header *e = (struct ether_header*) buf;
@@ -260,6 +264,7 @@ _pkt_Send(int s, char flg, u_int crypted, u_char *data
 	hdr->ansh_flg = flg;
 	hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
 	hdr->ansh_nonce = htonl(crypted);
+	hdr->ansh_seq = htonl(seq);
 	hdr->ansh_crc = 0;
 	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
 
@@ -278,13 +283,13 @@ _pkt_Send(int s, char flg, u_int crypted, u_char *data
 }
 
 int
-pktSend(int s, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea)
+pktSend(int s, u_int seq, char flg, u_int crypted, u_char *data, int datlen, struct ether_addr *ea)
 {
 	int wlen, ret = 0;
 	u_char *pos = data;
 
 	while (datlen > -1) {
-		wlen = _pkt_Send(s, flg, crypted, pos, (datlen > 512) ? 512 : datlen, ea);
+		wlen = _pkt_Send(s, seq, flg, crypted, pos, (datlen > 512) ? 512 : datlen, ea);
 		if (wlen == -1)
 			return -1;
 		else {
@@ -298,7 +303,7 @@ pktSend(int s, char flg, u_int crypted, u_char *data, 
 }
 
 static char
-_pkt_Recv(u_char * __restrict buf, int rlen, u_int * __restrict crypted, 
+_pkt_Recv(u_char * __restrict buf, int rlen, u_int * __restrict seq, u_int * __restrict crypted, 
 		u_char * __restrict data, int * __restrict datlen, 
 		u_char ** __restrict next, int * __restrict nextlen)
 {
@@ -357,21 +362,25 @@ _pkt_Recv(u_char * __restrict buf, int rlen, u_int * _
 		memcpy(data, buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr), *datlen);
 	}
 
+	if (seq)
+		*seq = ntohl(hdr->ansh_seq);
 	return hdr->ansh_flg;
 }
 
 char
-pktRecv(int s, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen, 
-		struct ether_header *eth)
+pktRecv(int s, u_int * __restrict seq, u_int * __restrict crypted, u_char * __restrict data, 
+		int * __restrict datlen, struct ether_header *eth)
 {
-	u_char *buf, *next, *pos, *ptr;
+	u_char *buf, *next, *ptr, *pos = data;
 	int nextlen, rlen, buflen, ptrlen;
 	char flg;
 	struct bpf_hdr *bpf;
 	struct ether_header *e;
 
-	if (!eth || !datlen)
+	if (!eth || !data || !datlen)
 		return ANSH_FLG_ERR;
+	else
+		memset(data, 0, *datlen);
 
 	if (!(buf = malloc(*datlen))) {
 		ERR("malloc() #%d - %s", errno, strerror(errno));
@@ -399,9 +408,7 @@ pktRecv(int s, u_int * __restrict crypted, u_char * __
 
 	ptr = next = buf;
 	ptrlen = nextlen = rlen;
-	pos = data;	
-	buflen = *datlen;
-	if ((flg = _pkt_Recv(ptr, ptrlen, crypted, pos, &buflen, &next, &nextlen)) == -1) {
+	if ((flg = _pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen)) == -1) {
 		free(buf);
 		return ANSH_FLG_ERR;
 	} else {
@@ -410,8 +417,9 @@ pktRecv(int s, u_int * __restrict crypted, u_char * __
 		ptr = next;
 		ptrlen = nextlen;
 	}
+	/* get additional packets from buffer */
 	while (next && nextlen > 0)
-		if (_pkt_Recv(ptr, ptrlen, crypted, pos, &buflen, &next, &nextlen) == -1)
+		if (_pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen) == -1)
 			break;
 		else {
 			pos += buflen;
@@ -421,6 +429,7 @@ pktRecv(int s, u_int * __restrict crypted, u_char * __
 		}
 
 	free(buf);
+
 	return flg;
 }