/*************************************************************************
 * (C) 2011 AITNET - Sofia/Bulgaria - <office@aitnet.org>
 *  by Michael Pounov <misho@elwix.org>
 *
 * $Author: misho $
 * $Id: utils.c,v 1.1.1.1.2.6 2011/10/13 14:29:30 misho Exp $
 *
 *************************************************************************/
#include "global.h"


void
Get1stEth(char *psDev, int devlen)
{
	struct ifaddrs *ifa;

	assert(psDev);
	assert(devlen > 0);

	getifaddrs(&ifa);
	strlcpy(psDev, ifa->ifa_name, devlen);
	freeifaddrs(ifa);
}

int
PrepareL2(const char *psDev, int *bpflen)
{
	int h, n = 1;
	register int i;
	char szStr[STRSIZ];
	struct ifreq ifr;
	struct bpf_program fcode = { 0 };
	struct bpf_insn insns[] = {
		BPF_STMT(BPF_LD + BPF_H + BPF_ABS, 12),
		BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ANSH_ID, 0, 1),
		BPF_STMT(BPF_RET + BPF_K, -1),
		BPF_STMT(BPF_RET + BPF_K, 0),
	};

	FTRACE(3);
	assert(psDev);

	fcode.bf_len = sizeof(insns) / sizeof(struct bpf_insn);
	fcode.bf_insns = insns;

	for (i = 0; i < 10; i++) {
		memset(szStr, 0, sizeof szStr);
		snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);
		h = open(szStr, O_RDWR);
		if (h > 2)
			break;
	}
	if (h < 3) {
		printf("Error:: open bpf %s #%d - %s\n", szStr, errno, strerror(errno));
		return -1;
	}

	if (ioctl(h, BIOCIMMEDIATE, &n) == -1) {
		printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
		close(h);
		return -1;
	}
	strlcpy(ifr.ifr_name, psDev, sizeof ifr.ifr_name);
	if (ioctl(h, BIOCSETIF, &ifr) == -1) {
		printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
		close(h);
		return -1;
	}
	if (ioctl(h, BIOCSETF, &fcode) == -1) {
		printf("Error:: set filter interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
		close(h);
		return -1;
	}
	if (ioctl(h, BIOCGBLEN, bpflen) == -1) {
		printf("Error:: get interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno));
		close(h);
		return -1;
	}

	/*
	n = fcntl(h, F_GETFL);
	fcntl(h, F_SETFL, n | O_NONBLOCK);
	*/

	VERB(3) LOG("Openned device handle %d with bpf buflen %d", h, *bpflen);
	return h;
}

int
PrepareL3(const struct sockaddr *sa, int *bpflen)
{
	int h, n = 1;

	FTRACE(3);
	assert(sa);

	h = socket(sa->sa_family, SOCK_RAW, IPPROTO_ICMP);
	if (h == -1) {
		printf("Error:: Cant open raw socket #%d - %s\n", errno, strerror(errno));
		return -1;
	}
	/*
	if (setsockopt(h, SOL_SOCKET, SO_REUSEADDR, &n, sizeof n) == -1) {
		printf("Error:: Cant set raw socket #%d - %s\n", errno, strerror(errno));
		close(h);
		return -1;
	}
	*/
	if (bind(h, sa, sizeof(struct sockaddr)) == -1) {
		printf("Error:: Cant bind to raw socket #%d - %s\n", errno, strerror(errno));
		close(h);
		return -1;
	}

	n = fcntl(h, F_GETFL);
	fcntl(h, F_SETFL, n | O_NONBLOCK);

	*bpflen = USHRT_MAX;
	VERB(3) LOG("Openned socket handle %d", h);
	return h;
}

char
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;
	struct ansh_hdr *hdr;
	u_char buf[USHRT_MAX] = { 0 };
	u_int crc;

	ret = recvfrom(s, buf, sizeof buf, 0, sa, salen);
	if (ret == -1) {
		ERR("Receive recvfrom() #%d - %s", errno, strerror(errno));
		return ANSH_FLG_ERR;
	} else
		VERB(4) LOG("Get packet with len=%d", ret);

	/* check header len */
	if (ret < (sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr))) {
		VERB(1) LOG("Discard packet too short %d ...", ret);
		return ANSH_FLG_ERR;
	} else
		icmp = (struct icmp*) (buf + sizeof(struct ip));

	/* check echo magic ansh code */
	if (icmp->icmp_type != ICMP_ECHOREPLY || icmp->icmp_code != ANSH_CODE) {
		VERB(3) LOG("Packet isnt for me %d ... icmp_code=%d", ret, icmp->icmp_code);
		return ANSH_FLG_ERR;
	} else
		hdr = (struct ansh_hdr*) (buf + sizeof(struct ip) + sizeof(struct icmp));

	/* check version and total size of packet */
	if (hdr->ansh_ver != ANSH_VERSION) {
		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;
	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
	if (crc != hdr->ansh_crc) {
		VERB(3) LOG("Packet with wrong crc ...");
		return ANSH_FLG_ERR;
	}

	/* copy data */
	if (data && datlen) {
		memset(data, 0, *datlen);
		*datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
		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_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;
	struct ansh_hdr *hdr;
	int ret = 0;

	assert(data);
	if ((sizeof buf - sizeof(struct icmp) + sizeof(struct ansh_hdr)) < datlen)
		return ANSH_FLG_ERR;

	icmp = (struct icmp*) buf;
	hdr = (struct ansh_hdr*) (buf + sizeof(struct icmp));
	pos = buf + sizeof(struct icmp) + sizeof(struct ansh_hdr);

	memcpy(pos, data, datlen);

	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_seq = htonl(seq);
	hdr->ansh_crc = 0;
	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));

	icmp->icmp_type = ICMP_ECHOREPLY;
	icmp->icmp_code = ANSH_CODE;
	icmp->icmp_id = htons(id);
	icmp->icmp_seq = htons(datlen);
	icmp->icmp_cksum = 0;
	icmp->icmp_cksum = crcIP(buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);

	if ((ret = sendto(s, buf, sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen, 
					0, sa, salen)) == -1) {
		ERR("Send sendto() #%d - %s", errno, strerror(errno));
		return ANSH_FLG_ERR;
	} else
		VERB(4) LOG("Put packet with len=%d", ret);
	if (ret != sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen) {
		VERB(3) LOG("Sended data %d is different from source data len %d", ret, 
				sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);
		return ANSH_FLG_ERR;
	}

	return ret;
}

static int
_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;
	struct ansh_hdr *hdr;
	int ret = 0;

	assert(data);
	if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)
		return ANSH_FLG_ERR;

	e->ether_type = ntohs(ANSH_ID);
	memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN);
	hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);
	pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);

	memcpy(pos, data, datlen);

	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_seq = htonl(seq);
	hdr->ansh_crc = 0;
	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));

	if ((ret = write(s, buf, ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen)) == -1) {
		ERR("Send packet() #%d - %s", errno, strerror(errno));
		return ANSH_FLG_ERR;
	} else
		VERB(4) LOG("Put packet with len=%d", ret);
	if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) {
		VERB(3) LOG("Sended data %d is different from source data len %d", ret, 
				ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen);
		return ANSH_FLG_ERR;
	}

	return ret;
}

int
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, seq, flg, crypted, pos, (datlen > 512) ? 512 : datlen, ea);
		if (wlen == -1)
			return -1;
		else {
			pos += wlen;
			datlen -= wlen;
			ret += wlen;
		}
	}

	return ret;
}

static char
_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)
{
	int bias;
	struct bpf_hdr *bpf;
	struct ansh_hdr *hdr;
	u_int crc;

	if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
		VERB(1) LOG("Discard packet too short %d ...", rlen);
		return ANSH_FLG_ERR;
	} else {
		bpf = (struct bpf_hdr*) buf;
		hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);
	}

	/* slice readed data to packets */
	if ((bias = BPF_WORDALIGN(bpf->bh_hdrlen + bpf->bh_caplen)) < rlen) {
		*next = buf + bias;
		*nextlen = rlen - bias;
	} else {
		*next = NULL;
		*nextlen = 0;
	}

	/* check version and total size of packet */
	if (hdr->ansh_ver != ANSH_VERSION) {
		VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);
		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;
	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));
	if (crc != hdr->ansh_crc) {
		VERB(3) LOG("Packet with wrong crc ...");
		return ANSH_FLG_ERR;
	}

	/* select data */
	if (data) {
		*datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
		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 seq, u_int * __restrict crypted, u_char * __restrict data, 
		int * __restrict datlen, struct ether_header *eth)
{
	u_char *buf, *next, *ptr, *pos = data;
	int nextlen, rlen, buflen, ptrlen;
	char flg;
	struct bpf_hdr *bpf;
	struct ether_header *e;

	if (!eth || !data || !datlen)
		return ANSH_FLG_ERR;
	else
		memset(data, 0, *datlen);

	if (!(buf = malloc(*datlen))) {
		ERR("malloc() #%d - %s", errno, strerror(errno));
		return ANSH_FLG_ERR;
	}

	rlen = read(s, buf, *datlen);
	if (rlen == -1) {
		ERR("Receive packet() #%d - %s", errno, strerror(errno));
		free(buf);
		return ANSH_FLG_ERR;
	} else
		VERB(4) LOG("Get packet with len=%d", rlen);

	/* check header len */
	if (rlen < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
		VERB(1) LOG("Discard packet too short %d ...", rlen);
		free(buf);
		return ANSH_FLG_ERR;
	} else {
		bpf = (struct bpf_hdr*) buf;
		e = (struct ether_header*) (buf + bpf->bh_hdrlen);
		memcpy(eth, e, ETHER_HDR_LEN);
	}

	ptr = next = buf;
	ptrlen = nextlen = rlen;
	if ((flg = _pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen)) == -1) {
		free(buf);
		return ANSH_FLG_ERR;
	} else {
		pos += buflen;
		*datlen = buflen;
		ptr = next;
		ptrlen = nextlen;
	}
	/* get additional packets from buffer */
	while (next && nextlen > 0)
		if (_pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen) == -1)
			break;
		else {
			pos += buflen;
			*datlen += buflen;
			ptr = next;
			ptrlen = nextlen;
		}

	free(buf);

	return flg;
}

void *
TOfunc(sched_task_t *task)
{
	struct tagProc *proc;

	FTRACE(3);

	/* not found argument, drop data */
	if (!(proc = TASK_ARG(task)))
		return (void*) -1;

	if (proc->proc_pid)
		kill(proc->proc_pid, SIGTERM);

	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;
}