--- ansh/src/utils.c	2011/10/04 22:37:46	1.1.1.1
+++ ansh/src/utils.c	2011/10/10 09:11:48	1.1.1.1.2.2
@@ -3,7 +3,7 @@
  *  by Michael Pounov <misho@elwix.org>
  *
  * $Author: misho $
- * $Id: utils.c,v 1.1.1.1 2011/10/04 22:37:46 misho Exp $
+ * $Id: utils.c,v 1.1.1.1.2.2 2011/10/10 09:11:48 misho Exp $
  *
  *************************************************************************/
 #include "global.h"
@@ -101,7 +101,7 @@ PrepareL3(const struct sockaddr *sa, int *bpflen)
 }
 
 char
-icmpRecv(int s, u_short * __restrict id, u_char * __restrict data, 
+icmpRecv(int s, u_short * __restrict id, u_int * __restrict crypted, u_char * __restrict data, 
 		int * __restrict datlen, struct sockaddr *sa, socklen_t *salen)
 {
 	int ret = 0;
@@ -136,7 +136,19 @@ icmpRecv(int s, u_short * __restrict id, u_char * __re
 		VERB(3) LOG("Packet with wrong version ...");
 		return ANSH_FLG_ERR;
 	}
+	if (crypted) {
+		if (hdr->ansh_nonce && !*crypted) {
+			VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ...");
+			return ANSH_FLG_ERR;
+		}
+		if (!hdr->ansh_nonce && *crypted) {
+			VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ...");
+			return ANSH_FLG_ERR;
+		}
 
+		*crypted = ntohl(hdr->ansh_nonce);
+	}
+
 	/* check crc of packet */
 	crc = hdr->ansh_crc;
 	hdr->ansh_crc ^= hdr->ansh_crc;
@@ -159,7 +171,7 @@ icmpRecv(int s, u_short * __restrict id, u_char * __re
 }
 
 int
-icmpSend(int s, u_short id, char flg, u_char *data, int datlen, struct sockaddr *sa, socklen_t salen)
+icmpSend(int s, 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;
@@ -179,6 +191,7 @@ icmpSend(int s, u_short id, char flg, u_char *data, in
 	hdr->ansh_ver = ANSH_VERSION;
 	hdr->ansh_flg = flg;
 	hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
+	hdr->ansh_nonce = htonl(crypted);
 	hdr->ansh_crc = 0;
 	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
 
@@ -205,7 +218,7 @@ icmpSend(int s, u_short id, char flg, u_char *data, in
 }
 
 int
-pktSend(int s, u_short id, char flg, u_char *data, int datlen, struct ether_addr *ea)
+pktSend(int s, u_short id, 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;
@@ -226,6 +239,7 @@ pktSend(int s, u_short id, char flg, u_char *data, int
 	hdr->ansh_ver = ANSH_VERSION;
 	hdr->ansh_flg = flg;
 	hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
+	hdr->ansh_nonce = htonl(crypted);
 	hdr->ansh_crc = 0;
 	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
 
@@ -244,7 +258,8 @@ pktSend(int s, u_short id, char flg, u_char *data, int
 }
 
 char
-pktRecv(int s, u_char * __restrict data, int * __restrict datlen, struct ether_header *eth)
+pktRecv(int s, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen, 
+		struct ether_header *eth)
 {
 	int ret = 0;
 	struct bpf_hdr *bpf;
@@ -287,6 +302,19 @@ pktRecv(int s, u_char * __restrict data, int * __restr
 		free(buf);
 		return ANSH_FLG_ERR;
 	}
+	if (crypted) {
+		if (hdr->ansh_nonce && !*crypted) {
+			VERB(3) LOG("Channel INSECURED:: Crypted communication not supported at this moment ...");
+			return ANSH_FLG_ERR;
+		}
+		if (!hdr->ansh_nonce && *crypted) {
+			VERB(3) LOG("Channel SECURED:: Plain text communication not supported at this moment ...");
+			return ANSH_FLG_ERR;
+		}
+
+		*crypted = ntohl(hdr->ansh_nonce);
+	}
+
 	/* check crc of packet */
 	crc = hdr->ansh_crc;
 	hdr->ansh_crc ^= hdr->ansh_crc;
@@ -326,3 +354,24 @@ TOfunc(sched_task_t *task)
 	return NULL;
 }
 
+u_char *
+cryptBuffer(u_char *buf, int rlen, u_int ctr)
+{
+	u_char *str, ivec[AES_BLOCK_SIZE] = { 0 };
+	u_int rctr = htonl(ctr);
+
+	FTRACE(3);
+
+	if (!buf)
+		return NULL;
+
+	memcpy(ivec, &ctr, sizeof ctr);
+	memcpy(ivec + 4, &rctr, sizeof rctr);
+	memcpy(ivec + 8, &ctr, sizeof ctr);
+	memcpy(ivec + 12, &rctr, sizeof rctr);
+
+	if (io_ctr_AES(buf, rlen, &str, (u_char*) "_ansh_ELWIX_", ivec) == -1)
+		return NULL;
+
+	return str;
+}