msgsimul.c

mobile ip 在linux下的一种实现

/* $Id: msgsimul.c,v 1.7 2001/08/22 07:57:11 ban Exp $
 * Simulator for test messages
 *
 * Dynamic hierarchial IP tunnel
 * Copyright (C) 2000-2001, Dynamics group
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation. See README and COPYING for
 * more details.
 */

#include <sys/types.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <string.h>
#include <stdio.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <time.h>
#include <sys/types.h>
#include <netinet/ip.h>
#include <netinet/udp.h>

#include "message.h"
#include "msgparser.h"
#include "md5_mac.h"
#include "util.h"


/* Command line format:
 * <seconds from start> <command>{<field1>=<value1> ...} [option1]{..} ...
 * commands:
 *   send{dstip,dstport,srcip,srcport}
 *     req{opts,lifetime,home,ha,coa,saveid}
 *     rep{code,lifetime,home,ha,useid}
 *     mhauth{spi,secret}
 *     mfauth{spi,secret}
 *     fhauth{spi,secret}
 *     raw{data}
 *   note{txt}
 *
 * (see msgsimul1.data for an example)
 */


extern int opt_debug;

static __u32 nextid = 0;
static struct timeval start;


struct parse_data {
	char *name; /* command or option */
	char *next; /* next token */

	char *field; /* parameter name */
	char *value; /* parameter value */

	int was_last; /* more parameters? */
};


static int is_space(char c)
{
	if (c == ' ' || c == '\t' || c == 13 || c == 10)
		return 1;
	return 0;
}

/**
 * parse_init:
 * @parse: 
 * @buf: command line buffer
 * 
 * Places the command in parse->name. parse->next points to the next
 * parameter. parse->was_last is 0 if more parameters are available.
 *  
 * Returns:
 * 0 = no more entries
 * 1 = entry available, name in parse->name */
int parse_init(struct parse_data *parse, char *buf)
{
	char *t;

	t = buf;
	while (is_space(*t)) t++;
	parse->name = t;
	while (*t != '\0' && *t != '{') t++;
	if (*t == '\0') {
		return 0;
	} else {
		*t = '\0';
		t++;
		parse->next = t;
	}
	parse->was_last = 0;
	return 1;
}

/* Returns:
 * -1 = invalid input data
 *  0 = no more entries
 *  1 = entry available in {parse->field, parse->value}
 */
int parse_get_next(struct parse_data *parse)
{
	if (parse->was_last)
		return 0;
	parse->was_last = 0;
	while (is_space(*parse->next))
		parse->next++;
	if (*parse->next == '\0')
		return -1;
	if (*parse->next == '}') {
		parse->next++;
		return 0;
	}

	parse->field = parse->next;
	while (*parse->next != '\0' && *parse->next != '=')
		parse->next++;
	if (*parse->next == '\0')
		return -1;
	*parse->next = '\0';
	parse->next++;
	while (is_space(*parse->next))
		parse->next++;
	if (*parse->next == '"') {
		parse->next++;
		parse->value = parse->next;
		while (*parse->next != '\0' && *parse->next != '"')
			parse->next++;
		if (*parse->next == '\0')
			return -1;
		*parse->next = '\0';
		parse->next++;
	} else {
		parse->value = parse->next;
		while (*parse->next != '\0' && !is_space(*parse->next) &&
		       *parse->next != '}')
			parse->next++;
		if (*parse->next == '\0')
			return -1;
		if (*parse->next == '}')
			parse->was_last = 1;
		*parse->next = '\0';
		parse->next++;
	}

	return 1;
}


struct saved_id {
	int num;
	__u32 id[2];

	struct saved_id *next;
};

struct saved_id *saved_ids = NULL;

static void saveid(int num, __u32 id[])
{
	struct saved_id *entry;
	printf("\tsaveid[%i] = %08x:%08x\n", num, id[0], id[1]);
	entry = (struct saved_id *) malloc(sizeof(struct saved_id));
	if (entry == NULL) {
		fprintf(stderr, "saveid: out of memory\n");
		return;
	}
	entry->num = num;
	entry->id[0] = id[0];
	entry->id[1] = id[1];
	entry->next = saved_ids;
	saved_ids = entry;
}

static void useid(int num, __u32 id[])
{
	struct saved_id *tmp = saved_ids;

	printf("\tuseid[%i]\n", num);

	while (tmp != NULL) {
		if (tmp->num == num) {
			id[0] = tmp->id[0];
			id[1] = tmp->id[1];
			return;
		}
		tmp = tmp->next;
	}

	fprintf(stderr, "useid - saved id=%i not found\n", num);
}


#define RECVBUF 8192
void get_msg(int s)
{
	struct sockaddr_in addr;
	char buf[RECVBUF];
	int n, len;
	struct msg_extensions ext;
	struct timeval tv, diff;

	gettimeofday(&tv, NULL);
	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("\nIncoming message at %lu.%06lu (start + %lu.%06lu)\n\n",
	       tv.tv_sec, tv.tv_usec, diff.tv_sec, diff.tv_usec);

	len = sizeof(addr);
	n = recvfrom(s, buf, RECVBUF, 0, (struct sockaddr *) &addr, &len);
	if (n < 0) {
		perror("recvfrom");
		return;
	}
	printf("Received %i bytes from %s:%i\n", n,
	       inet_ntoa(addr.sin_addr), ntohs(addr.sin_port));

	parse_msg(buf, n, &ext);
}


int add_req(char *buf, int left, struct parse_data *parse)
{
	struct reg_req *req;

	if (sizeof(struct reg_req) > left) {
		fprintf(stderr, "add_req - buffer full\n");
		return 0;
	}

	req = (struct reg_req *) buf;
	memset(req, 0, sizeof(struct reg_req));
	req->type = REG_REQ;
	req->id[0] = htonl(time(NULL) + UNIX_NTP_DIFF);
	req->id[1] = nextid++;

	while (parse_get_next(parse) == 1) {
		if (strcmp(parse->field, "lifetime") == 0)
			req->lifetime = htons(atoi(parse->value));
		else if (strcmp(parse->field, "opts") == 0)
			req->opts = atoi(parse->value);
		else if (strcmp(parse->field, "home") == 0)
			inet_aton(parse->value, &req->home_addr);
		else if (strcmp(parse->field, "ha") == 0)
			inet_aton(parse->value, &req->ha_addr);
		else if (strcmp(parse->field, "coa") == 0)
			inet_aton(parse->value, &req->co_addr);
		else if (strcmp(parse->field, "saveid") == 0)
			saveid(atoi(parse->value), req->id);
		else {
			fprintf(stderr, "add_req - unknown field '%s'\n",
				parse->field);
			return 0;
		}
	}
	return sizeof(struct reg_req);
}

int add_rep(char *buf, int left, struct parse_data *parse)
{
	struct reg_rep *rep;

	if (sizeof(struct reg_rep) > left) {
		fprintf(stderr, "add_rep - buffer full\n");
		return 0;
	}

	rep = (struct reg_rep *) buf;
	memset(rep, 0, sizeof(struct reg_rep));
	rep->type = REG_REP;
	rep->id[0] = htonl(time(NULL) + UNIX_NTP_DIFF);
	rep->id[1] = nextid++;

	while (parse_get_next(parse) == 1) {
		if (strcmp(parse->field, "lifetime") == 0)
			rep->lifetime = htons(atoi(parse->value));
		else if (strcmp(parse->field, "code") == 0)
			rep->code = atoi(parse->value);
		else if (strcmp(parse->field, "home") == 0)
			inet_aton(parse->value, &rep->home_addr);
		else if (strcmp(parse->field, "ha") == 0)
			inet_aton(parse->value, &rep->ha_addr);
		else if (strcmp(parse->field, "useid") == 0)
			useid(atoi(parse->value), rep->id);
		else {
			fprintf(stderr, "add_rep - unknown field '%s'\n",
				parse->field);
			return 0;
		}
	}

	return sizeof(struct reg_rep);
}

int add_auth(int type, char *start, char *buf, int left,
	     struct parse_data *parse)
{
	struct msg_auth *auth;
	char *secret = NULL;

	if (sizeof(struct msg_auth) + MD5_MAC_LEN > left) {
		fprintf(stderr, "add_auth - buffer full\n");
		return 0;
	}

	auth = (struct msg_auth *) buf;
	memset(auth, 0, sizeof(struct msg_auth));
	auth->type = type;
	auth->length = SPI_LEN + MD5_MAC_LEN;

	while (parse_get_next(parse) == 1) {
		if (strcmp(parse->field, "spi") == 0)
			auth->spi = htonl(atoi(parse->value));
		else if (strcmp(parse->field, "secret") == 0)
			secret = parse->value;
		else {
			fprintf(stderr, "add_auth - unknown field '%s'\n",
				parse->field);
			return 0;
		}
	}
	md5_mac(secret == NULL ?
		(unsigned char *) "" : (unsigned char *) secret,
		secret == NULL ? 0 : strlen(secret),
		(unsigned char *) start, buf - start + 6, MSG_AUTH_DATA(auth));

	return sizeof(struct msg_auth) + MD5_MAC_LEN;
}


int add_raw(char *buf, int left, struct parse_data *parse)
{
	char *pos = buf, *rpos;
	int i, count;

	while (parse_get_next(parse) == 1) {
		if (strcmp(parse->field, "data") == 0) {
			count = strlen(parse->value) / 2;
			if (count > left) {
				fprintf(stderr, "add_raw - buffer full\n");
				return 0;
			}
			rpos = parse->value;
			for (i = 0; i < count; i++) {
				*pos = get_hex_digit(*rpos) * 16 +
					get_hex_digit(*(rpos + 1));
				pos++;
				rpos += 2;
			}
		} else {
			fprintf(stderr, "add_raw - unknown field '%s'\n",
				parse->field);
			return 0;
		}
	}

	return pos - buf;
}


#define SENDBUF 8192

void send_forced(char *buf, int len, struct sockaddr_in *addr,
		 struct sockaddr_in *src)
{
	int s, n, rawlen, one = 1;
	char rawbuf[SENDBUF + sizeof(struct iphdr) + sizeof(struct udphdr)];
	struct iphdr *ip;
	struct udphdr *udp;


        s = socket(AF_INET, SOCK_RAW, IPPROTO_RAW);
	if (s < 0) {
                if (errno == EPERM)
                        fprintf(stderr,
				"Forced src addr requires root privileges\n");
                perror("socket");
                return;
        }

	if (setsockopt(s,  SOL_IP, IP_HDRINCL, &one, sizeof(one)) < 0) {
		perror("setsockopt - IP_HDRINCL");
		return;
	}

	ip = (struct iphdr *) rawbuf;
	memset(ip, 0, sizeof(struct iphdr));
	ip->ihl = 5;
	ip->version = 4;
	ip->ttl = 255;
	ip->tot_len = htons(sizeof(struct udphdr) + len);
	ip->protocol = IPPROTO_UDP;
	ip->saddr = src->sin_addr.s_addr;
	ip->daddr = addr->sin_addr.s_addr;
	
	udp = (struct udphdr *) (ip + 1);
	udp->source = src->sin_port;
	udp->dest = addr->sin_port;
	udp->len = htons(len + 8);
	udp->check = 0; /* FIX */

	memcpy(udp + 1, buf, len);

	rawlen = len + sizeof(struct iphdr) + sizeof(struct udphdr);
	addr->sin_port = 0; /* required for raw sockets */
	n = sendto(s, rawbuf, rawlen, 0, (struct sockaddr *) addr,
		   sizeof(*addr));
	if (n < 0)
		perror("sendto");
	else if (n != rawlen) {
		fprintf(stderr, "sent only %i/%i bytes\n", n, rawlen);
	}
}


void handle_send(int s, struct parse_data *parse)
{
	struct sockaddr_in addr, src;
	char buf[SENDBUF], *pos;
	int n, forced_src = 0;
	struct msg_extensions ext;

	printf("handling SEND\n");

	memset(&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
	inet_aton("127.0.0.1", &addr.sin_addr);
	addr.sin_port = htons(434);

	memset(&src, 0, sizeof(src));
	src.sin_family = AF_INET;
	inet_aton("127.0.0.1", &src.sin_addr);
	src.sin_port = htons(434);

	while (parse_get_next(parse) == 1) {
		if (strcmp(parse->field, "dstport") == 0)
			addr.sin_port = htons(atoi(parse->value));
		else if (strcmp(parse->field, "dstip") == 0)
			inet_aton(parse->value, &addr.sin_addr);
		else if (strcmp(parse->field, "srcport") == 0) {
			forced_src = 1;
			src.sin_port = htons(atoi(parse->value));
		} else if (strcmp(parse->field, "srcip") == 0) {
			forced_src = 1;
			inet_aton(parse->value, &src.sin_addr);
		} else {
			fprintf(stderr, "Unknown send option '%s', value "
				"'%s'\n", parse->field, parse->value);
			return;
		}
	}

	pos = buf;
	while (parse_init(parse, parse->next) == 1) {
		int left = pos - buf + SENDBUF;
		printf("\tdata: %s\n", parse->name);
		if (strcmp(parse->name, "req") == 0)
			pos += add_req(pos, left, parse);
		else if (strcmp(parse->name, "rep") == 0)
			pos += add_rep(pos, left, parse);
		else if (strcmp(parse->name, "mhauth") == 0)
			pos += add_auth(MH_AUTH, buf, pos, left, parse);
		else if (strcmp(parse->name, "mfauth") == 0)
			pos += add_auth(MF_AUTH, buf, pos, left, parse);
		else if (strcmp(parse->name, "fhauth") == 0)
			pos += add_auth(FH_AUTH, buf, pos, left, parse);
		else if (strcmp(parse->name, "raw") == 0)
			pos += add_raw(pos, left, parse);
		else {
			fprintf(stderr, "Unknown send item '%s'\n",
				parse->name);
			return;
		}
	}

	if (parse_msg(buf, pos - buf, &ext) != 0) {
		fprintf(stderr, "\tInvalid message!\n");
	}

	printf("sending %i bytes to %s:%i\n", pos - buf,
	       inet_ntoa(addr.sin_addr), ntohs(addr.sin_port));

	if (forced_src)
		send_forced(buf, pos - buf, &addr, &src);
	else {
		n = sendto(s, buf, pos - buf, 0, (struct sockaddr *) &addr,
			   sizeof(addr));
		if (n < 0) {
			perror("sendto");
		} else if (n != pos - buf) {
			fprintf(stderr, "sent only %i/%i bytes\n", n,
				pos - buf);
		}
	}
}


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(int s, char *command)
{
	char *pos, *t;
	struct timeval next, tv, diff;
	fd_set rfds;
	struct parse_data parse;

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

	*pos = '\0';
	t = strchr(command, '.');
	if (t == NULL) {
		next.tv_sec = start.tv_sec + atoi(command);
		next.tv_usec = start.tv_usec;
	} else {
		int slen;
		*t = '\0';
		next.tv_sec = start.tv_sec + atoi(command);
		t++;
		slen = strlen(t);
		if (slen > 6)
			t[6] = '\0';
		next.tv_usec = start.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++;
		}
	}

	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)\n",
	       next.tv_sec, next.tv_usec, diff.tv_sec, diff.tv_usec);

	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(s, &rfds);
		select(s + 1, &rfds, NULL, NULL, &diff);
		if (FD_ISSET(s, &rfds))
			get_msg(s);
	}

	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);
	pos++;
	/* parse: command{params..} .. */
	parse_init(&parse, pos);
	if (strcmp(parse.name, "send") == 0)
		handle_send(s, &parse);
	else if (strcmp(parse.name, "note") == 0)
		handle_note(&parse);
	else {
		fprintf(stderr, "unknown command[%s]\n", pos);
		return;
	}
}

int main(int argc, char *argv[])
{
	int s;
	FILE *f;
	char buf[10240];

	opt_debug = 1;

	if (argc < 2) {
		fprintf(stderr, "msgsimul <command file>\n");
		exit(-1);
	}

	s = socket(AF_INET, SOCK_DGRAM, 0);
	if (s < 0) {
		perror("socket");
		exit(-1);
	}

	f = fopen(argv[1], "r");
	if (f == NULL) {
		fprintf(stderr, "Could not open command file '%s'\n", argv[1]);
		exit(-1);
	}

	gettimeofday(&start, NULL);
	printf("Simulation for command file '%s' started at %lu.%06lu\n",
	       argv[1], start.tv_sec, start.tv_usec);

	while (fgets(buf, sizeof(buf), f)) {
		char *pos = buf;
		while (is_space(*pos)) pos++;
		if (*pos == ';' || *pos == '\0')
			continue; /* skip comments and empty lines */
		handle(s, pos);
	}

	fclose(f);
	close(s);

	return 0;
}