Return to utils.c CVS log

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

Initial revision

/*************************************************************************
 * (C) 2011 AITNET - Sofia/Bulgaria - <office@aitnet.org>
 *  by Michael Pounov <misho@elwix.org>
 *
 * $Author: misho $
 * $Id: utils.c,v 1.1 2011/10/04 22:37:46 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;

	FTRACE(3);
	assert(psDev);

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

	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, BIOCIMMEDIATE, &n) == -1) {
		printf("Error:: set 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;
	}

	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_short * __restrict id, 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;
	}

	/* 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 (id)
		*id = ntohs(icmp->icmp_id);
	return hdr->ansh_flg;
}

int
icmpSend(int s, u_short id, char flg, 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_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;
}

int
pktSend(int s, u_short id, char flg, 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 = htons(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_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;
}

char
pktRecv(int s, u_char * __restrict data, int * __restrict datlen, struct ether_header *eth)
{
	int ret = 0;
	struct bpf_hdr *bpf;
	struct ether_header *e;
	struct ansh_hdr *hdr;
	u_char *buf;
	u_int crc;

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

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

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

	/* check header len */
	if (ret < (sizeof(struct bpf_hdr) + ETHER_HDR_LEN + sizeof(struct ansh_hdr))) {
		VERB(1) LOG("Discard packet too short %d ...", ret);
		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);
		hdr = (struct ansh_hdr*) (buf + bpf->bh_hdrlen + ETHER_HDR_LEN);
	}

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

	/* 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);
	return (char) ret;
}

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