stun.c

STUN（RFC3489）客户端的简单实现

#include <stdlib.h>
#include <time.h>
#include <assert.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdio.h>
#include <errno.h>

#include "stun.h"


static char *encode16(char* buf, UInt16 data)
{
	UInt16 ndata = htons(data);
	memcpy(buf, &ndata, sizeof(UInt16));
	return buf + sizeof(UInt16);
}

static char *encode32(char* buf, UInt32 data)
{
	UInt32 ndata = htonl(data);
	memcpy(buf, &ndata, sizeof(UInt32));
	return buf + sizeof(UInt32);
}

UInt128 createTransctionID (void)
{
	UInt128 id;
	UInt32 i,r;
	struct timeval tv;

	gettimeofday(&tv, NULL);
	srand((int)time(0)+(int)tv.tv_usec);
	for(i=0; i<16; i+=4)
	{
		assert(i+3 < 16);
		r = rand();
		id.octet[i+0] = r>>0;
		id.octet[i+1] = r>>8;
		id.octet[i+2] = r>>16;
		id.octet[i+3] = r>>24;
	}
	return id;
}

void initIdArray(UInt128 *ID, int length)
{
	int i,j;
	
	for(i=0; i<length; i++)
	{
		for(j=0; j<16; j++)
		{
			ID->octet[j] = 0;
		}
		ID++;
	}
}

bool checkTransctionID(UInt128 id, UInt128 *ID, int count)
{
	assert(count > 0);
	int i;
//	int j;
//	printf("count is %d\n",count);
		
	for(i=0; i<count; i++)
	{
//		printf("\n");
//		for(j=0; j<16; j++)printf("%x",ID->octet[j]);
//		printf("\n");
//		for(j=0;j<16;j++)printf("%x",id.octet[j]);
		if(memcmp(ID,&id,16) == 0)return true;
		else ID++;
	}

	return false;
}
	
void buildSimBindingRequest(char *buf, UInt128 *ID)
{	
	StunMsgHdr header;
	char *p;

	header.msgType = BindingRequest;
	header.msgLength = 0;
	header.id = createTransctionID();
	*ID = header.id;
	p = encode16(buf, header.msgType);
	p = encode16(p, header.msgLength);
	memcpy(p, &header.id, sizeof(header.id));
}

void buildTestBindingRequest(char *buf, bool change_ip, bool change_port,
                              UInt128 *ID)
{
	StunMsgHdr header;
	StunAtrHdr atr_head;
	StunAtrChangeRequest atr;
	char *p;
	//write stun header
	header.msgType = BindingRequest;
	header.msgLength = 8;
	header.id = createTransctionID();
	*ID = header.id;
	p = encode16(buf, header.msgType);
	p = encode16(p, header.msgLength);
	memcpy(p, &header.id, sizeof(header.id));
	p = p+sizeof(header.id);

	//write stun attributes
	atr_head.type = ChangeRequest;
	atr_head.length = 4;
	atr.value = 0x00000000;
	if(change_ip)
	{
		atr.value = atr.value|0x00000004;
	}
	if(change_port)
	{
		atr.value = atr.value|0x00000002;
	}
	atr.value = htonl(atr.value);
	p = encode16(p, atr_head.type);
	p = encode16(p, atr_head.length); 
	memcpy(p, &atr.value, sizeof(atr.value));
	//for test
/*	int i;
//	for(i=0; i<28;i++)
//	{
//		printf("%x",*(buf+i));
//		if(i == 19)printf("\n");
	}*/
}
	
/********************************************/
/* buf: point to Resquest you want to send  */
/* fd : file descriptor returned by socket  */
/* ip : address to send.(Network-endian)    */
/*port: port to send.(Network-endian)       */
/*  l : length of request you send          */
/********************************************/
bool sendBindingRequest(char *buf, int fd, UInt32 ip, UInt16 port, UInt16 l)
{
	int nbyte;
	struct sockaddr_in server_addr;

	memset(&server_addr,0,sizeof(server_addr));
	server_addr.sin_family = AF_INET;
	server_addr.sin_addr.s_addr = ip;
	server_addr.sin_port = port;	
	
	nbyte = sendto(fd, buf, l, 0, (struct sockaddr *)&server_addr,
                    sizeof(server_addr));
	if(nbyte == -1)
	{
		fprintf(stderr,"send failed:%s",strerror(errno));
		return false;
	}
	else if(nbyte < l)
	{
		fprintf(stdout,"you want to send % bytes,but send % bytes only!\n",
                        l, nbyte);
		return false;
	}

	return true;
}

 /****************************************************/
 /* buf: point to Response you have received         */
 /* fd : file descriptor returned by socket          */
 /*  l : max length you can receive                  */
 /****************************************************/
bool receiveResponse(char *buf, int fd, UInt16 l)
{
	int nbyte;
	int addr_len;
	struct sockaddr_in server_addr;
	
	addr_len = sizeof(server_addr);
	memset(&server_addr,0,addr_len);
//	server_addr.sin_family = AF_INET;
//	server_addr.sin_addr.s_addr = ip;
//	server_addr.sin_port = port;	

	nbyte = recvfrom(fd, buf, l, 0,(struct sockaddr *)&server_addr,&addr_len);
	if(nbyte == -1)
	{
		return false;
	}
	printf("receive data from %s/%d\n",inet_ntoa(server_addr.sin_addr),
            ntohs(server_addr.sin_port));
		
	return true;
}

//if ipv6,rewrite this function
bool parseAtrAddress(const char *p, UInt16 length, StunAtrAddress4 *addr)
{	
	if(length != 8)return false;

	memcpy(&addr->ipv4.port, p+2, 2);
	memcpy(&addr->ipv4.addr, p+4, 4);
	addr->ipv4.port = ntohs(addr->ipv4.port);
	addr->ipv4.addr = ntohl(addr->ipv4.addr);
	
	return true;
}

/************************************************/
/*Unknown response return false                 */
/*incorrect response return false               */
/*correct response rertun true                  */
/************************************************/
bool parseBindingResponse(char *buf, Address4 *addr, UInt128 *ID, int count)
{	
	StunMessage msg;
	char *p=buf;
	int len;
	// init message
	msg.hasMappedAddress    = false;
	msg.hasSourceAddress    = false;
	msg.hasChangedAddress   = false;
	msg.hasMessageIntegrity = false;
	msg.hasReflectedFrom	= false;

	memcpy(&msg.msgHdr.msgType, p, 2);
	memcpy(&msg.msgHdr.msgLength, p+2, 2);
	memcpy(&msg.msgHdr.id, p+4, 16);
	if(!checkTransctionID(msg.msgHdr.id,ID,count))
	{
		printf("incorrect ID!\n");
		return false;
	}
	p = p+20;
	msg.msgHdr.msgType = ntohs(msg.msgHdr.msgType);
	msg.msgHdr.msgLength = ntohs(msg.msgHdr.msgLength);
	len = msg.msgHdr.msgLength;
	printf("message type:%x\t length:%d\n", msg.msgHdr.msgType,len);
	
	StunAtrHdr atr_head;
	if(msg.msgHdr.msgType == BindingResponse)
	{	
		while(len >0)
		{
			memcpy(&atr_head.type, p, 2);
			memcpy(&atr_head.length, p+2, 2);
			atr_head.type = htons(atr_head.type);
			atr_head.length = htons(atr_head.length);
			p = p+4;
			switch(atr_head.type)
			{
				case MappedAddress:
					msg.hasMappedAddress = true;
					if(!parseAtrAddress(p,atr_head.length,
							      &msg.mappedAddress))
					{
						printf("parse MappedAddress failed!\n");
						return false;
					}
					break;
				case SourceAddress:
					msg.hasSourceAddress = true;
					if(!parseAtrAddress(p,atr_head.length,
								&msg.sourceAddress))
					{
						printf("parse SourceAddress failed!\n");
						return false;
					}
					break;
				case ChangedAddress:
					msg.hasChangedAddress = true;
					if(!parseAtrAddress(p,atr_head.length,
								&msg.changedAddress))
					{
						printf("parse ChangedAddress failed!\n");
						return false;
				}
					break;
				case MessageIntegrity:break;
				case ReflectedFrom:break;
				default:
					fprintf(stdout,"Unknown attribute:%x\n",atr_head.type);
					break;
			}
			len = len - 4 -atr_head.length;
			p = p+atr_head.length;
		}
		//output information to file
		if(msg.hasMappedAddress)
		{	
			addr->mapped_ip = msg.mappedAddress.ipv4.addr;
			addr->mapped_port = msg.mappedAddress.ipv4.port;
			UInt8 *q = (UInt8 *)&msg.mappedAddress.ipv4.addr;
			printf("MappedAddress:%d.%d.%d.%d:%d\n",*(q+3),*(q+2),*(q+1),
                    *q,msg.mappedAddress.ipv4.port);
		}
		else
		{
			printf("no MappedAddress attributes\n");
		}

		if(msg.hasSourceAddress)
		{
			UInt8 *q = (UInt8 *)&msg.sourceAddress.ipv4.addr;
			printf("SourceAddress:%d.%d.%d.%d:%d\n",*(q+3),*(q+2),*(q+1),
				*q,msg.sourceAddress.ipv4.port);
		}
		else
		{
			printf("no SourceAddress attribute!\n");
		}

		if(msg.hasChangedAddress)
		{
			addr->changed_ip = msg.changedAddress.ipv4.addr;
			addr->changed_port = msg.changedAddress.ipv4.port;
			UInt8 *q =(UInt8 *)&msg.changedAddress.ipv4.addr;
			printf("ChangedAddress:%d.%d.%d.%d:%d\n",*(q+3),*(q+2),*(q+1),
				*q,msg.changedAddress.ipv4.port);
		}
		else
		{
			printf("no ChangedAddress attribute!");
		}
		
		if(msg.hasMessageIntegrity)
		{
			//do something
		}
		
		if(msg.hasReflectedFrom)
		{
			//do something
		}
	}//end of parse binding response
	
	else if(msg.msgHdr.msgType == BindingErrorResponse)
	{
		printf("error code\n");		
	}
	else
	{
		fprintf(stdout,"Unknown stun type!\n");
		return false;
	}
	printf("\n");

	return true;
}

/****************************************************************/
/*    bufReq : point to the content of Request                  */
/*    bufRes : point to the memory receive Response             */
/*      fd   : file descriptor returned by socket               */
/* change_ip : change ip or not                                 */
/*change_port: change port or not                               */
/*      ip   : address to send or receive                       */
/*     port  : port to send or receive                          */
/*      l    : data length to send                              */
/*    addr   : save useful ip or port                           */
/****************************************************************/
bool sendMessage(char *bufReq, char *bufRes,  int fd, bool change_ip,
		 bool change_port, UInt32 ip, UInt16 port, UInt16 l, Address4 *addr)
{
	int flag;
	int state;
	int args;
	int count = 0;
	const int timePoint[]={0,100,300,700,1500,3100,4700,6300,7900};
	UInt128 ID[10];
	int timeuse = 0;
	struct timeval tv1,tv2;

	initIdArray(ID,10);		
	flag = 1;
	state = Build;
	gettimeofday(&tv1,NULL);
	while(flag)
	{
		switch(state)
		{
			case Build:
				buildTestBindingRequest(bufReq,change_ip,change_port,&ID[count]);
				sendBindingRequest(bufReq,fd,ip,port,l);
				count++;
				state = WaitResponse;	
				break;
			case WaitResponse:
				args = fcntl(fd, F_GETFL,0);
				fcntl(fd,F_SETFL,args|O_NONBLOCK);		
				if(receiveResponse(bufRes,fd,256))
				{
					state = ParseResponse;
					break;
				}
				gettimeofday(&tv2,NULL);
				timeuse = (tv2.tv_sec-tv1.tv_sec)*1000+
					    (tv2.tv_usec-tv1.tv_usec)/1000;
	
				if(timeuse > 9500)
				{
					printf("didn't receive response in 9.5s\n");
					return false;
				}
				else if(count < 9 && timeuse > timePoint[count])
				{
					state = Build;
				}
				break;
			case ParseResponse:
				if(parseBindingResponse(bufRes,addr,ID,count))
				{
					state = EndProcess;
				}
				else
				{
					printf("receive error response!\n");
					state = WaitResponse;
				}
				break;
			case EndProcess:
				flag = 0;
				break;
			default:
				break;
		}
	}
	return true;
}