netsimul.c

mobile ip 在linux下的一种实现

	ip->version = 4;
	ip->ttl = 255;
	ip->protocol = IPPROTO_UDP;
	ip->daddr = ip->saddr = htonl((127 << 24) | 1);

	while (parse_get_next(parse) == 1) {
		if (strcmp(parse->field, "dstip") == 0) {
			inet_aton(parse->value, &addr);
			ip->daddr = addr.s_addr;
		} else if (strcmp(parse->field, "srcip") == 0) {
			inet_aton(parse->value, &addr);
			ip->saddr = addr.s_addr;
		} else if (strcmp(parse->field, "proto") == 0) {
			ip->protocol = dyn_atoi(parse->value);
		} else if (strcmp(parse->field, "ttl") == 0) {
			ip->ttl = dyn_atoi(parse->value);
		}
	}


	pos = (char *) (ip + 1);
	while (parse_init(parse, parse->next) == 1) {
		int left = pos - buf + SENDBUF;
		printf("\tip-data: %s\n", parse->name);
		if (strcmp(parse->name, "ip") == 0)
			pos += add_ip(pos, left, parse);
		else if (strcmp(parse->name, "udp") == 0)
			pos += add_udp(pos, left, parse);
		else if (strcmp(parse->name, "icmp") == 0)
			pos += add_icmp(pos, left, parse);
		else
			printf("\tunknown ip data\n");
	}

	ip->tot_len = htons(pos - buf);
	ip->check = 0;
	ip->check = ip_checksum((unsigned char *) ip, sizeof(struct iphdr));

	return pos - buf;
}

void handle_send(struct parse_data *parse)
{
	int s = fd[0];
	char buf[SENDBUF], *pos;
	int n, dsthw_set = 0;
	struct ethhdr *eth;

	printf("handling SEND\n");

	memset(buf, 0, sizeof(buf));
	eth = (struct ethhdr *) buf;
	eth->h_proto = htons(ETH_P_IP);

	while (parse_get_next(parse) == 1) {
		if (strcmp(parse->field, "srchw") == 0) {
			if (ether_atohw(parse->value, eth->h_source) < 0)
				return;
		} else if (strcmp(parse->field, "dsthw") == 0) {
			if (ether_atohw(parse->value, eth->h_dest) < 0)
				return;
			dsthw_set = 1;
		} else if (strcmp(parse->field, "proto") == 0) {
			eth->h_proto = htons(dyn_atoi(parse->value));
		} else if (strcmp(parse->field, "dev") == 0) {
			if (strcmp(parse->value, "tap0") == 0)
				s = fd[0];
			else if (strcmp(parse->value, "tap1") == 0)
				s = fd[1];
			else if (strcmp(parse->value, "tap2") == 0)
				s = fd[2];
			else {
				fprintf(stderr, "Unknown interface '%s'\n",
					parse->value);
				return;
			}
		} else {
			fprintf(stderr, "Unknown send option '%s', value "
				"'%s'\n", parse->field, parse->value);
			return;
		}
	}

	if (!dsthw_set) {
		if (s == fd[0])
			ether_atohw("00:00:00:00:00:01", eth->h_dest);
		else if (s == fd[1])
			ether_atohw("00:00:00:00:00:02", eth->h_dest);
		else if (s == fd[2])
			ether_atohw("00:00:00:00:00:03", eth->h_dest);
	}

	pos = (char *) (eth + 1);
	while (parse_init(parse, parse->next) == 1) {
		int left = pos - buf + SENDBUF;
		printf("\tsend-data: %s\n", parse->name);
		if (strcmp(parse->name, "ip") == 0)
			pos += add_ip(pos, left, parse);
		else
			printf("\tunknown send data\n");
	}



	printf("sending %i bytes\n", pos - buf);

	n = write(s, buf, pos - buf);
	if (n < 0)
		perror("write");
	else if (n != pos - buf)
		fprintf(stderr, "sent only %i/%i bytes\n", n, pos - buf);
}


void handle_expect(struct parse_data *parse)
{
	printf("handling EXPECT\n");

	if (expect_received)
		printf("\texpected packet received\n");
	else {
		printf("\texpected packet was NOT received\n");
		expect_error++;
	}

	printf("\texpected: %s\n", current_command);
}


void handle_reject(struct parse_data *parse)
{
	printf("handling REJECT\n");

	if (!expect_received)
		printf("\texpected (reject) packet not received (OK)\n");
	else {
		printf("\texpected (reject) packet was received (FAIL)\n");
		reject_error++;
	}

	printf("\texpected (reject): %s\n", current_command);
}


void handle_note(struct parse_data *parse)
{
	printf("handling NOTE\n");

	while (parse_get_next(parse) == 1) {
		if (strcmp(parse->field, "txt") == 0)
			printf("\t%s\n", parse->value);
		else {
			fprintf(stderr, "Unknown note option '%s', value "
				"'%s'\n", parse->field, parse->value);
			return;
		}
	}
	printf("\n");
}


void handle_exec(struct parse_data *parse)
{
	printf("handling EXEC\n");

	while (parse_get_next(parse) == 1) {
		if (strcmp(parse->field, "cmd") == 0) {
			printf("running(%s)\n", parse->value);
			system(parse->value);
		} else {
			fprintf(stderr, "Unknown note option '%s', value "
				"'%s'\n", parse->field, parse->value);
			return;
		}
	}
	printf("\n");
}


void handle(char *command)
{
	char *pos, *t;
	struct timeval next, tv, diff;
	fd_set rfds;
	struct parse_data parse;
	static struct timeval prev = {0, 0};

	pos = command;
	while (*pos != '\0' && !is_space(*pos)) pos++;
	if (*pos == '\0') {
		fprintf(stderr, "Empty command[%s]\n", command);
		return;
	}

	*pos = '\0';

	pos++;
	strncpy(current_command, pos, PARSE_BUF_SIZE);
	/* parse: command{params..} .. */
	parse_init(&parse, pos);
	if (strcmp(parse.name, "expect") == 0) {
		current_cmd_expect = current_expect = 1;
		expect_received = 0;
		reject_cmd = 0;
	} else 	if (strcmp(parse.name, "reject") == 0) {
		current_cmd_expect = current_expect = 1;
		expect_received = 0;
		reject_cmd = 1;
	} else
		reject_cmd = current_cmd_expect = current_expect = 0;

	tv = start;
	if (command[0] == '+') {
		command++;
		tv = prev;
	}

	t = strchr(command, '.');
	if (t == NULL) {
		next.tv_sec = tv.tv_sec + atoi(command);
		next.tv_usec = tv.tv_usec;
	} else {
		int slen;
		*t = '\0';
		next.tv_sec = tv.tv_sec + atoi(command);
		t++;
		while (*t == '0')
			t++;
		slen = strlen(t);
		if (slen > 6)
			t[6] = '\0';
		next.tv_usec = tv.tv_usec;
		switch (slen) {
		case 1: next.tv_usec += atoi(t) * 100000; break;
		case 2: next.tv_usec += atoi(t) * 10000; break;
		case 3: next.tv_usec += atoi(t) * 1000; break;
		case 4: next.tv_usec += atoi(t) * 100; break;
		case 5: next.tv_usec += atoi(t) * 10; break;
		default: next.tv_usec += atoi(t); break;
		}
		if (next.tv_usec > 999999) {
			next.tv_usec -= 1000000;
			next.tv_sec++;
		}
	}
	prev = next;

	diff.tv_sec = next.tv_sec - start.tv_sec;
	diff.tv_usec = next.tv_usec - start.tv_usec;
	if (diff.tv_usec < 0) {
		diff.tv_sec--;
		diff.tv_usec += 1000000;
	}

	printf("next event at %lu.%06lu (start + %lu.%06lu) [%s]\n",
	       next.tv_sec, next.tv_usec, diff.tv_sec, diff.tv_usec,
	       parse.name);

	for (;;) {
		gettimeofday(&tv, NULL);
		diff.tv_sec = next.tv_sec - tv.tv_sec;
		diff.tv_usec = next.tv_usec - tv.tv_usec;
		if (diff.tv_usec < 0) {
			diff.tv_usec += 1000000;
			diff.tv_sec--;
		}
		if (diff.tv_sec < 0 || (diff.tv_sec == 0 && diff.tv_usec <= 0))
			break;

		FD_ZERO(&rfds);
		FD_SET(fd[0], &rfds);
		FD_SET(fd[1], &rfds);
		FD_SET(fd[2], &rfds);
		select(max_fd, &rfds, NULL, NULL, &diff);
		if (FD_ISSET(fd[0], &rfds))
			process(fd[0], "tap0");
		if (FD_ISSET(fd[1], &rfds))
			process(fd[1], "tap1");
		if (FD_ISSET(fd[2], &rfds))
			process(fd[2], "tap2");
	}

	diff.tv_sec = tv.tv_sec - start.tv_sec;
	diff.tv_usec = tv.tv_usec - start.tv_usec;
	if (diff.tv_usec < 0) {
		diff.tv_sec--;
		diff.tv_usec += 1000000;
	}
	printf("\nrunning command at %lu.%06lu (start + %lu.%06lu)\n\n",
	       tv.tv_sec, tv.tv_usec, diff.tv_sec, diff.tv_usec);
	if (strcmp(parse.name, "send") == 0)
		handle_send(&parse);
	else if (strcmp(parse.name, "expect") == 0)
		handle_expect(&parse);
	else if (strcmp(parse.name, "reject") == 0)
		handle_reject(&parse);
	else if (strcmp(parse.name, "note") == 0)
		handle_note(&parse);
	else if (strcmp(parse.name, "exec") == 0)
		handle_exec(&parse);
	else {
		fprintf(stderr, "unknown command[%s]\n", pos);
		return;
	}
}


int tun_alloc(char *dev)
{
    struct ifreq ifr;
    int fd, err;

    if ((fd = open("/dev/net/tun", O_RDWR)) < 0) {
	    perror("open(/dev/net/tun)");
	    return -1;
    }

    memset(&ifr, 0, sizeof(ifr));
    ifr.ifr_flags = IFF_TAP | IFF_NO_PI;
    if (dev && dev[0] != '\0')
	    dynamics_strlcpy(ifr.ifr_name, dev, IFNAMSIZ);

    if ((err = ioctl(fd, TUNSETIFF, (void *) &ifr)) < 0) {
	    perror("ioctl(TUNSETIFF)");
	    close(fd);
	    return err;
    }

    if (dev)
	    dynamics_strlcpy(dev, ifr.ifr_name, IFNAMSIZ);
    return fd;
}              


void process_router_adv(char *buf, int len)
{
	struct router_adv *adv;
	struct router_adv_router *router;
	int i, left;
	unsigned char *pos;
	struct agent_adv_ext *advext;
	struct vendor_ext_header *vend_ext;
	unsigned int u;

	printf("\tRouteradv:");

	if (len < sizeof(struct router_adv)) {
		printf("\n");
		return;
	}

	adv = (struct router_adv *) buf;
	left = len - sizeof(struct router_adv);
	printf(" num_addr=%i entry_size=%i lifetime=%i",
	       adv->num_addr, adv->entry_size, ntohs(adv->lifetime));

	router = (struct router_adv_router *) (adv + 1);
	for (i = 0; i < adv->num_addr; i++) {
		if (left < sizeof(struct router_adv_router)) {
			printf("\trouter_adv_router underflow\n");
			return;
		}
		left -= sizeof(struct router_adv_router);
		printf(" router(addr=%s, pref=%i)",
		       inet_ntoa(*(struct in_addr *)&router->router_addr),
		       ntohl(router->pref_level));
		router++;
	}
	printf("\n");

	if (left == 0)
		return;

	pos = (unsigned char *) router;
	if (*pos != AGENT_ADVERTISEMENT_EXTENSION_TYPE) {
		printf("\tUnknown router advertisement extension type %i\n",
		       *pos);
		return;
	}

	while (left > 0) {
		if (*pos != ONE_BYTE_PADDING &&
		    (left < 2 || left - 2 < pos[1])) {
			printf("\tExtension underflow\n");
			return;
		}
		switch (*pos) {
		case AGENT_ADVERTISEMENT_EXTENSION_TYPE:
			if (left < sizeof(struct agent_adv_ext)) {
				printf("\tAgentadv underflow\n");
				return;
			}
			advext = (struct agent_adv_ext *) pos;
			u = ntohs(advext->opts);
			printf("\tAgentadv: type=%i length=%i seq=%i "
			       "reg_lifetime=%i opts=0x%04X",
			       advext->type, advext->length,
			       ntohs(advext->seq), ntohs(advext->reg_lifetime),
			       u);
			if (u & AGENT_ADV_REGISTRATION_REQUIRED)
				printf(" [RegReq]");
			if (u & AGENT_ADV_BUSY)
				printf(" [Busy]");
			if (u & AGENT_ADV_HOME_AGENT)
				printf(" [HA]");
			if (u & AGENT_ADV_FOREIGN_AGENT)
				printf(" [FA]");
			if (u & AGENT_ADV_MINIMAL_ENCAPSULATION)
				printf(" [MinEncaps]");
			if (u & AGENT_ADV_GRE_ENCAPSULATION)
				printf(" [GRE]");
			if (u & AGENT_ADV_VJ_HDR_COMPRESSION)
				printf(" [VJ]");
			if (u & AGENT_ADV_BIDIR_TUNNELING)
				printf(" [BiDir]");
			u &= ~(AGENT_ADV_REGISTRATION_REQUIRED |
			       AGENT_ADV_BUSY |
			       AGENT_ADV_HOME_AGENT |
			       AGENT_ADV_FOREIGN_AGENT |
			       AGENT_ADV_MINIMAL_ENCAPSULATION |
			       AGENT_ADV_GRE_ENCAPSULATION |
			       AGENT_ADV_VJ_HDR_COMPRESSION |
			       AGENT_ADV_BIDIR_TUNNELING);
			if (u)
				printf(" [Unknown: 0x%04X]", u);
			printf("\n");
			break;

		case VENDOR_EXT_TYPE2:
			if (left < sizeof(struct vendor_ext_header)) {
				printf("\tVendor ext underflow\n");
				return;
			}
			vend_ext = (struct vendor_ext_header *) pos;
			printf("\tVendorExt: type=%i length=%i reserved=%i "
			       "vendor_id=%i sub_type=%i\n",
			       vend_ext->type, vend_ext->length,
			       vend_ext->reserved, ntohl(vend_ext->vendor_id),
			       ntohs(vend_ext->sub_type));
			if (ntohl(vend_ext->vendor_id) != VENDOR_ID_DYNAMICS ||
			    ntohs(vend_ext->sub_type) !=
			    VENDOR_EXT_DYNAMICS_AGENTADV) {
				printf("\tUnknown vendor extension\n");
			} else if (left >= sizeof(struct agent_adv_dynamics)) {
				struct agent_adv_dynamics *advd =
					(struct agent_adv_dynamics *) vend_ext;
				printf("\tDynamics adv: version=%i opts=0x%02X"
				       " reserved2=%i\n",
				       advd->version, advd->opts,
				       advd->reserved2);
			}
			break;

		case ONE_BYTE_PADDING:
			printf("\tOne byte padding\n");
			break;

		default:
			printf("\tUnknown extension type %i\n", *pos);
			break;
		}

		if (*pos == ONE_BYTE_PADDING) {
			left--;
			pos++;
		} else {
			left -= 2 + pos[1];
			pos += 2 + pos[1];
		}

	}
}

void process_icmp(char *buf, int len)
{
	struct icmphdr *icmp;

	if (len < 4)
		return;

	icmp = (struct icmphdr *) buf;

	printf("\tICMP: type=%i code=%i\n", icmp->type, icmp->code);

	switch (icmp->type) {
	case ICMP_ECHOREPLY:
		printf("\tEcho Reply\n");
		break;

	case ICMP_ECHO:
		printf("\tEcho Request\n");
		break;

	case ICMP_ROUTERADVERT:
		process_router_adv(buf, len);
		break;

	default:
		printf("\tUnknown ICMP type.\n");
		break;
	}
}


void check_auth(unsigned char *start, struct msg_auth *auth)
{
	char *secret = NULL;
	int alg = AUTH_ALG_MD5_PREFIX_SUFFIX;
	int spi = 0;

	if (auth == NULL) {
		printf("EXPECT: missing auth ext (%s)\n", expect_parse.name);
		current_expect = 0;
		return;
	}

	while (parse_get_next(&expect_parse) == 1) {
		if (strcmp(expect_parse.field, "spi") == 0) {
			spi = htonl(dyn_atoi(expect_parse.value));
			if (spi != auth->spi) {
				printf("EXPECT: spi does not match\n");
				current_expect = 0;
				break;
			}
		} else if (strcmp(expect_parse.field, "alg") == 0)
			alg = dyn_atoi(expect_parse.value);
		else if (strcmp(expect_parse.field, "secret") == 0)
			secret = expect_parse.value;
		else {
			fprintf(stderr, "check_auth - unknown field '%s'\n",
				expect_parse.field);
			current_expect = 0;
			break;
		}
	}

	if (!auth_check(alg, secret ? secret : "", secret ? strlen(secret) : 0,
			start, auth)) {
		printf("EXPECT: authentication does not match\n");
		current_expect = 0;
	}

	if (current_expect) {
		if (parse_init(&expect_parse, expect_parse.next) == 0) {
			printf("Expected packet received.\n");
			current_expect = 0;
			expect_received = 1;
		}
	}
}


void check_no_ext(const char *title, void *ext_ptr)
{
	while (parse_get_next(&expect_parse) == 1);

	if (ext_ptr != NULL) {
		printf("EXPECT: no-%s, but packet has the extension\n",
		       title);
		current_expect = 0;
	} else {
		if (parse_init(&expect_parse, expect_parse.next) == 0) {
			printf("Expected packet received.\n");
			current_expect = 0;
			expect_received = 1;
		}
	}
}


void process_udp(char *buf, int len)
{
	struct udphdr *udp;
	struct msg_extensions ext;
	struct in_addr addr;

	if (len < sizeof(struct udphdr))
		return;

	udp = (struct udphdr *) buf;
	printf("\tUDP: srcport=%i dstport=%i len=%i check=0x%04X\n",
	       ntohs(udp->source), ntohs(udp->dest), ntohs(udp->len),