build_tftp.c

裸板的tftp客户端

#include "./include/build_tftp.h"
#include "./include/myout.h"
char FileName[]={"u-boot.bin"};
char Prot[]={"octet"};
static U8 	HostMac[], ClientMac[]={0x00,0x11,0x22,0x33,0x44,0x55} ;
static U8 	HostIp[]={192,168,0,1}, ClientIp[]={192,168,0,111};
static U16	HostPort=69, SrPort=0x8002;

U16 check_sum(U16 *pwData,U32 nSize)
{
	U32 nChksum=0;
	while(nSize>1){
		nChksum+=*pwData++;
		nSize-=sizeof(U16);
	}
	if(nSize)
		nChksum+=*(U8*)pwData;
	nChksum=(nChksum>>16)+(nChksum&0xffff);
	nChksum+=(nChksum>>16);
	return (U16)(~nChksum);
}

inline void swap(U8 *a, U8 *b)
{
	U8 tmpch;
	tmpch=*a;
	*a=*b;
	*b=tmpch;

}

int build_tftp_packet(U8 *PacketPage, U8 *HMac, U16 HPort, U16 BlockNumber, U32 PacketType)
{
	int i, j, totalLen=0, useLen=0, macLen=14, ipLen=20, vudpLen=12, udpLen=8, tmpInt;
	U16 *tmpdata6;

	//UseData
	tmpInt=macLen+ipLen+vudpLen+udpLen;
	if(PacketType == 1)			// file request packetpage
	{
		PacketPage[tmpInt]=0x00;
		PacketPage[tmpInt+1]=0x01;
		for(i=0;FileName[i]!='\0';i++)
			PacketPage[2+tmpInt+i]=FileName[i];
		for(j=0;Prot[j]!='\0';j++)
			PacketPage[i+3+j+tmpInt]=Prot[j];
		useLen=i+j+4;
	}
	else if(PacketType == 4)
	{
		PacketPage[tmpInt]=0x00;
		PacketPage[tmpInt+1]=0x04;
		tmpdata6=(U16 *)(PacketPage+tmpInt+2);
		*tmpdata6=BlockNumber;
		swap(&PacketPage[tmpInt+2], &PacketPage[tmpInt+3]);
		useLen=4;
	}

	//MacHead build
	for(i = 0; i < 6; i++)
	{
		if(1==PacketType){
			HostMac[i]=PacketPage[i]=HMac[i];
		}
		else
			PacketPage[i]=HostMac[i];
		PacketPage[i+6]=ClientMac[i];
	}
	PacketPage[2*i]=0x08;
	PacketPage[2*i+1]=0x00;

	// IpHead build
	totalLen=ipLen+udpLen+useLen;
	PacketPage[macLen]= 5<<0|4<<4;		//Ip headLen & version
	PacketPage[macLen+1]=0x00;		// tos
	tmpdata6=(U16 *)(PacketPage+macLen+2);
	*tmpdata6=useLen+udpLen+ipLen;		//totall len
	swap(&PacketPage[macLen+2], &PacketPage[macLen+3]);
	PacketPage[macLen+4]=0x0;			//unique identifier
	PacketPage[macLen+5]=0x0;
	PacketPage[macLen+6]=0x40;		// wFlags	
	PacketPage[macLen+7]=0x0;
	PacketPage[macLen+8]=0x40;		// life cycle
	PacketPage[macLen+9]=0x11;		// transmite type
	tmpdata6 = (U16 *)(PacketPage+macLen+10);
	*tmpdata6 = 0x00;				// checksum
	for(i=0;i<4;i++)
	{
		PacketPage[macLen+12+i]=ClientIp[i];	//source ip
		PacketPage[macLen+16+i]=HostIp[i];	//destination ip
	}
	tmpdata6 = (U16 *)(PacketPage+macLen+10);		//actual checksum
	*tmpdata6 = check_sum((U16 *)(PacketPage+macLen), 20);



	// VudpHead build
	tmpInt=macLen+ipLen;
	for(i=0;i<4;i++)
	{
		PacketPage[tmpInt+i]=ClientIp[i];	//source of Ip
		PacketPage[tmpInt+4+i]=HostIp[i];	//destination of Ip
	}
	PacketPage[tmpInt+8]=0x0;			//zero
	PacketPage[tmpInt+9]=0x11;
	PacketPage[tmpInt+10]=0x00;			//bProto
	PacketPage[tmpInt+11]=useLen+udpLen;		//wUdpLen

	//UdpHead build
	tmpInt=macLen+ipLen+vudpLen;
	tmpdata6=(U16 *)(PacketPage+tmpInt);
	*tmpdata6=SrPort;					//source of port
	tmpdata6=(U16 *)(PacketPage+tmpInt+2);
	if(1==BlockNumber)
		HostPort=*tmpdata6=HPort;
	else
		*tmpdata6=HostPort;
	tmpdata6=(U16 *)(PacketPage+tmpInt+4);
	*tmpdata6=useLen+udpLen;			//udp len
	PacketPage[tmpInt+6]=0x0;
	PacketPage[tmpInt+7]=0x0;
	tmpdata6 =(U16 *)(PacketPage+tmpInt+6);	//udp checksum
	*tmpdata6 =0x00;
	for(i=0;i<8;i+=2) 			//modify order
	{
		swap(&PacketPage[tmpInt+i], &PacketPage[tmpInt+i+1]);
	}
	*tmpdata6=check_sum((U16 *)(PacketPage+macLen+ipLen), useLen+udpLen+vudpLen);

	//clear VudpHead
	tmpInt = macLen+ipLen;
	for(i=0;i<udpLen+useLen;i++)
	{
		PacketPage[tmpInt+i]=PacketPage[tmpInt+vudpLen+i];
	}
	for(i=0;i<vudpLen;i++)
	{	
		PacketPage[tmpInt+udpLen+useLen+i]='\0';	
	}
	return macLen+ipLen+udpLen+useLen;
}