ansh/src/utils.c - diff

Return to utils.c CVS log

Up to [ELWIX - Embedded LightWeight unIX -] / ansh / src

Diff for /ansh/src/utils.c between versions 1.1.1.1 and 1.1.1.1.2.5

version 1.1.1.1, 2011/10/04 22:37:46	version 1.1.1.1.2.5, 2011/10/13 11:01:37
Line 29 PrepareL2(const char psDev, int bpflen)	Line 29 PrepareL2(const char psDev, int bpflen)
register int i;	register int i;
char szStr[STRSIZ];	char szStr[STRSIZ];
struct ifreq ifr;	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);	FTRACE(3);
assert(psDev);	assert(psDev);

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

for (i = 0; i < 10; i++) {	for (i = 0; i < 10; i++) {
memset(szStr, 0, sizeof szStr);	memset(szStr, 0, sizeof szStr);
snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);	snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);
Line 45 PrepareL2(const char psDev, int bpflen)	Line 55 PrepareL2(const char psDev, int bpflen)
return -1;	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);	strlcpy(ifr.ifr_name, psDev, sizeof ifr.ifr_name);
if (ioctl(h, BIOCSETIF, &ifr) == -1) {	if (ioctl(h, BIOCSETIF, &ifr) == -1) {
printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));	printf("Error:: bind interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
close(h);	close(h);
return -1;	return -1;
}	}
if (ioctl(h, BIOCIMMEDIATE, &n) == -1) {	if (ioctl(h, BIOCSETF, &fcode) == -1) {
printf("Error:: set interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));	printf("Error:: set filter interface %s to bpf #%d - %s\n", psDev, errno, strerror(errno));
close(h);	close(h);
return -1;	return -1;
}	}
Line 62 PrepareL2(const char psDev, int bpflen)	Line 77 PrepareL2(const char psDev, int bpflen)
return -1;	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);	VERB(3) LOG("Openned device handle %d with bpf buflen %d", h, *bpflen);
return h;	return h;
}	}
Line 101 PrepareL3(const struct sockaddr sa, int bpflen)	Line 121 PrepareL3(const struct sockaddr sa, int bpflen)
}	}

char	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 * __restrict datlen, struct sockaddr sa, socklen_t salen)
{	{
int ret = 0;	int ret = 0;
Line 136 icmpRecv(int s, u_short * __restrict id, u_char * __re	Line 156 icmpRecv(int s, u_short * __restrict id, u_char * __re
VERB(3) LOG("Packet with wrong version ...");	VERB(3) LOG("Packet with wrong version ...");
return ANSH_FLG_ERR;	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 */	/* check crc of packet */
crc = hdr->ansh_crc;	crc = hdr->ansh_crc;
hdr->ansh_crc ^= hdr->ansh_crc;	hdr->ansh_crc ^= hdr->ansh_crc;
Line 159 icmpRecv(int s, u_short * __restrict id, u_char * __re	Line 191 icmpRecv(int s, u_short * __restrict id, u_char * __re
}	}

int	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 };	u_char *pos, buf[USHRT_MAX] = { 0 };
struct icmp *icmp;	struct icmp *icmp;
Line 179 icmpSend(int s, u_short id, char flg, u_char *data, in	Line 211 icmpSend(int s, u_short id, char flg, u_char *data, in
hdr->ansh_ver = ANSH_VERSION;	hdr->ansh_ver = ANSH_VERSION;
hdr->ansh_flg = flg;	hdr->ansh_flg = flg;
hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));	hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
	hdr->ansh_nonce = htonl(crypted);
hdr->ansh_crc = 0;	hdr->ansh_crc = 0;
hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));

Line 204 icmpSend(int s, u_short id, char flg, u_char *data, in	Line 237 icmpSend(int s, u_short id, char flg, u_char *data, in
return ret;	return ret;
}	}

int	static int
pktSend(int s, u_short id, char flg, u_char data, int datlen, struct ether_addr ea)	_pkt_Send(int s, char flg, u_int crypted, u_char data, int datlen, struct ether_addr ea)
{	{
u_char *pos, buf[USHRT_MAX] = { 0 };	u_char *pos, buf[USHRT_MAX] = { 0 };
struct ether_header e = (struct ether_header) buf;	struct ether_header e = (struct ether_header) buf;
Line 216 pktSend(int s, u_short id, char flg, u_char *data, int	Line 249 pktSend(int s, u_short id, char flg, u_char *data, int
if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)	if ((sizeof buf - ETHER_HDR_LEN + sizeof(struct ansh_hdr)) < datlen)
return ANSH_FLG_ERR;	return ANSH_FLG_ERR;

e->ether_type = htons(id);	e->ether_type = ntohs(ANSH_ID);
memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN);	memcpy(e->ether_dhost, ea->octet, ETHER_ADDR_LEN);
hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);	hdr = (struct ansh_hdr*) (buf + ETHER_HDR_LEN);
pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);	pos = ((u_char*) hdr) + sizeof(struct ansh_hdr);
Line 226 pktSend(int s, u_short id, char flg, u_char *data, int	Line 259 pktSend(int s, u_short id, char flg, u_char *data, int
hdr->ansh_ver = ANSH_VERSION;	hdr->ansh_ver = ANSH_VERSION;
hdr->ansh_flg = flg;	hdr->ansh_flg = flg;
hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));	hdr->ansh_len = htons(datlen + sizeof(struct ansh_hdr));
	hdr->ansh_nonce = htonl(crypted);
hdr->ansh_crc = 0;	hdr->ansh_crc = 0;
hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));	hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));

Line 243 pktSend(int s, u_short id, char flg, u_char *data, int	Line 277 pktSend(int s, u_short id, char flg, u_char *data, int
return ret;	return ret;
}	}

char	int
pktRecv(int s, u_char * __restrict data, int * __restrict datlen, struct ether_header *eth)	pktSend(int s, char flg, u_int crypted, u_char data, int datlen, struct ether_addr ea)
{	{
int ret = 0;	int wlen, ret = 0;
	u_char *pos = data;

	while (datlen > -1) {
	wlen = _pkt_Send(s, 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 crypted,
	u_char * __restrict data, int * __restrict datlen,
	u_char ** __restrict next, int * __restrict nextlen)
	{
	int bias;
struct bpf_hdr *bpf;	struct bpf_hdr *bpf;
struct ether_header *e;
struct ansh_hdr *hdr;	struct ansh_hdr *hdr;
u_char *buf;
u_int crc;	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);
	}

	return hdr->ansh_flg;
	}

	char
	pktRecv(int s, u_int * __restrict crypted, u_char * __restrict data, int * __restrict datlen,
	struct ether_header *eth)
	{
	u_char buf, next, pos, ptr;
	int nextlen, rlen, buflen, ptrlen;
	char flg;
	struct bpf_hdr *bpf;
	struct ether_header *e;

if (!eth \|\| !datlen)	if (!eth \|\| !datlen)
return ANSH_FLG_ERR;	return ANSH_FLG_ERR;

Line 261 pktRecv(int s, u_char * __restrict data, int * __restr	Line 378 pktRecv(int s, u_char * __restrict data, int * __restr
return ANSH_FLG_ERR;	return ANSH_FLG_ERR;
}	}

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

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

/* check version and total size of packet */	ptr = next = buf;
if (hdr->ansh_ver != ANSH_VERSION) {	ptrlen = nextlen = rlen;
VERB(3) LOG("Packet with wrong version ... %d", hdr->ansh_ver);	pos = data;
	buflen = *datlen;
	if ((flg = _pkt_Recv(ptr, ptrlen, crypted, pos, &buflen, &next, &nextlen)) == -1) {
free(buf);	free(buf);
return ANSH_FLG_ERR;	return ANSH_FLG_ERR;
	} else {
	pos += buflen;
	*datlen = buflen;
	ptr = next;
	ptrlen = nextlen;
}	}
/* check crc of packet */	while (next && nextlen > 0)
crc = hdr->ansh_crc;	if (_pkt_Recv(ptr, ptrlen, crypted, pos, &buflen, &next, &nextlen) == -1)
hdr->ansh_crc ^= hdr->ansh_crc;	break;
hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));	else {
if (crc != hdr->ansh_crc) {	pos += buflen;
VERB(3) LOG("Packet with wrong crc ...");	*datlen += buflen;
free(buf);	ptr = next;
return ANSH_FLG_ERR;	ptrlen = nextlen;
}	}

/* copy data */
if (data) {
memset(data, 0, *datlen);
*datlen = ntohs(hdr->ansh_len) - sizeof(struct ansh_hdr);
memcpy(data, hdr + sizeof(struct ansh_hdr), *datlen);
}

ret = (char) hdr->ansh_flg;
free(buf);	free(buf);
return (char) ret;	return flg;
}	}

void *	void *
Line 326 TOfunc(sched_task_t *task)	Line 441 TOfunc(sched_task_t *task)
return NULL;	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;
	}

FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>

Removed from v.1.1.1.1
changed lines
	Added in v.1.1.1.1.2.5