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 and 1.2

version 1.1, 2011/10/04 22:37:46	version 1.2, 2011/10/17 20:14:02
Line 5	Line 5
* $Author$	* $Author$
* $Id$	* $Id$
*	*
*************************************************************************/	*************************************************************************
	The ELWIX and AITNET software is distributed under the following
	terms:

	All of the documentation and software included in the ELWIX and AITNET
	Releases is copyrighted by ELWIX - Sofia/Bulgaria <info@elwix.org>

	Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011
	by Michael Pounov <misho@elwix.org>. All rights reserved.

	Redistribution and use in source and binary forms, with or without
	modification, are permitted provided that the following conditions
	are met:
	1. Redistributions of source code must retain the above copyright
	notice, this list of conditions and the following disclaimer.
	2. Redistributions in binary form must reproduce the above copyright
	notice, this list of conditions and the following disclaimer in the
	documentation and/or other materials provided with the distribution.
	3. All advertising materials mentioning features or use of this software
	must display the following acknowledgement:
	This product includes software developed by Michael Pounov <misho@elwix.org>
	ELWIX - Embedded LightWeight unIX and its contributors.
	4. Neither the name of AITNET nor the names of its contributors
	may be used to endorse or promote products derived from this software
	without specific prior written permission.

	THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND
	ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
	FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
	DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
	OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
	HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
	LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
	OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
	SUCH DAMAGE.
	*/
#include "global.h"	#include "global.h"


Line 29 PrepareL2(const char psDev, int bpflen)	Line 66 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 92 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;
	}
	n = USHRT_MAX + 1;
	if (ioctl(h, BIOCSBLEN, &n) == -1) {
	printf("Error:: set buffer interface %s buffer length #%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;
}	}
if (ioctl(h, BIOCGBLEN, bpflen) == -1) {	if (ioctl(h, BIOCGBLEN, bpflen) == -1) {
printf("Error:: get interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno));	printf("Error:: get buffer interface %s buffer length #%d - %s\n", psDev, errno, strerror(errno));
close(h);	close(h);
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 162 PrepareL3(const struct sockaddr sa, int bpflen)
}	}

char	char
icmpRecv(int s, u_short * __restrict id, u_char * __restrict data,	icmpRecv(int s, u_int * __restrict seq, u_short * __restrict id, u_int * __restrict crypted,
int * __restrict datlen, struct sockaddr sa, socklen_t salen)	u_char * __restrict data, int * __restrict datlen, struct sockaddr sa, socklen_t salen)
{	{
int ret = 0;	int ret = 0;
struct icmp *icmp;	struct icmp *icmp;
Line 136 icmpRecv(int s, u_short * __restrict id, u_char * __re	Line 197 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;
	}
	if (ntohl(hdr->ansh_nonce) != *crypted)
	VERB(4) LOG("Detect change of nonce from %x to %x", *crypted, ntohl(hdr->ansh_nonce));

	*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 153 icmpRecv(int s, u_short * __restrict id, u_char * __re	Line 228 icmpRecv(int s, u_short * __restrict id, u_char * __re
memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen);	memcpy(data, buf + sizeof(struct ip) + sizeof(struct icmp) + sizeof(struct ansh_hdr), *datlen);
}	}

	if (seq)
	*seq = ntohl(hdr->ansh_seq);
if (id)	if (id)
*id = ntohs(icmp->icmp_id);	*id = ntohs(icmp->icmp_id);
return hdr->ansh_flg;	return hdr->ansh_flg;
}	}

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_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 };	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 257 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_seq = htonl(seq);
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 197 icmpSend(int s, u_short id, char flg, u_char *data, in	Line 277 icmpSend(int s, u_short id, char flg, u_char *data, in
VERB(4) LOG("Put packet with len=%d", ret);	VERB(4) LOG("Put packet with len=%d", ret);
if (ret != sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen) {	if (ret != sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen) {
VERB(3) LOG("Sended data %d is different from source data len %d", ret,	VERB(3) LOG("Sended data %d is different from source data len %d", ret,
sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen);	(int) (sizeof(struct icmp) + sizeof(struct ansh_hdr) + datlen));
return ANSH_FLG_ERR;	return ANSH_FLG_ERR;
}	}

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, u_int seq, char flg, u_int crypted, u_char data, int datlen, struct io_ether_addr ea)
{	{
u_char *pos, buf[USHRT_MAX] = { 0 };	u_char pos, str, buf[USHRT_MAX] = { 0 };
struct ether_header e = (struct ether_header) buf;	struct ether_header e = (struct ether_header) buf;
struct ansh_hdr *hdr;	struct ansh_hdr *hdr;
int ret = 0;	int ret = 0;
Line 216 pktSend(int s, u_short id, char flg, u_char *data, int	Line 296 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->ether_addr_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);

memcpy(pos, data, datlen);	memcpy(pos, data, datlen);

	if (Crypted) {
	str = cryptBuffer(pos, datlen, Crypted);
	if (str) {
	memcpy(pos, str, datlen);
	free(str);
	}
	}

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_seq = htonl(seq);
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 236 pktSend(int s, u_short id, char flg, u_char *data, int	Line 326 pktSend(int s, u_short id, char flg, u_char *data, int
VERB(4) LOG("Put packet with len=%d", ret);	VERB(4) LOG("Put packet with len=%d", ret);
if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) {	if (ret != ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen) {
VERB(3) LOG("Sended data %d is different from source data len %d", ret,	VERB(3) LOG("Sended data %d is different from source data len %d", ret,
ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen);	(int) (ETHER_HDR_LEN + sizeof(struct ansh_hdr) + datlen));
return ANSH_FLG_ERR;	return ANSH_FLG_ERR;
}	}

return ret;	return ret;
}	}

char	int
pktRecv(int s, u_char * __restrict data, int * __restrict datlen, struct ether_header *eth)	pktSend(int s, u_int seq, char flg, u_int crypted, u_char data, int datlen, struct io_ether_addr ea)
{	{
int ret = 0;	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 bpf_hdr *bpf;
struct ether_header *e;
struct ansh_hdr *hdr;	struct ansh_hdr *hdr;
u_char *buf;
u_int crc;	u_int crc;
	u_char *str;

if (!eth \|\| !datlen)	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;	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;
	}
	if (ntohl(hdr->ansh_nonce) != *crypted)
	VERB(4) LOG("Detect change of nonce from %x to %x", *crypted, ntohl(hdr->ansh_nonce));

	*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);
	if (Crypted) {
	str = cryptBuffer(buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr),
	*datlen, Crypted);
	if (str) {
	memcpy(buf + bpf->bh_hdrlen + ETHER_HDR_LEN + sizeof(struct ansh_hdr),
	str, *datlen);
	free(str);
	}
	}

	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))) {	if (!(buf = malloc(*datlen))) {
ERR("malloc() #%d - %s", errno, strerror(errno));	ERR("malloc() #%d - %s", errno, strerror(errno));
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);	if ((flg = _pkt_Recv(ptr, ptrlen, seq, 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 */	/* get additional packets from buffer */
crc = hdr->ansh_crc;	while (next && nextlen > 0)
hdr->ansh_crc ^= hdr->ansh_crc;	if (_pkt_Recv(ptr, ptrlen, seq, crypted, pos, &buflen, &next, &nextlen) == -1)
hdr->ansh_crc = htonl(crcAdler((u_char*) hdr, ntohs(hdr->ansh_len)));	break;
if (crc != hdr->ansh_crc) {	else {
VERB(3) LOG("Packet with wrong crc ...");	pos += buflen;
free(buf);	*datlen += buflen;
return ANSH_FLG_ERR;	ptr = next;
}	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 514 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*) Key, ivec) == -1)
	return NULL;

	return str;
	}

	int
	stopProcess(sched_root_task_t * __restrict root, proc_head_t * __restrict h, pid_t pid, sched_task_func_t func)
	{
	struct tagProc *p;

	FTRACE(3);

	SLIST_FOREACH(p, h, proc_next)
	if (p->proc_pid == pid) {
	break;
	}
	VERB(3) LOG("pid=%d found=%p\n", pid, p);
	if (!p)
	return 1;

	ioFreePTY(p->proc_pty, p->proc_ttyname);
	if (p->proc_pty)
	schedCancelby(root, NULL, CRITERIA_FD, (void*) ((intptr_t) p->proc_pty), NULL);

	p->proc_pty = 0;
	p->proc_pid = 0;
	p->proc_seq = 0;
	p->proc_flg = ANSH_FLG_EOF;
	p->proc_rlen_[FD2NET] = 0;

	schedCallOnce(root, func, p, p->proc_sock);
	return 0;
	}

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

Removed from v.1.1
changed lines
	Added in v.1.2