etherframe11.c

S3C44B0X接8019的源代码

#include <string.h>
#include <stdio.h>
#include "rtl8019as.h"
#include "..\inc\44b.h"
#include "..\inc\44blib.h"
#include "..\inc\def.h"

#include "NetProtocol.h"

BYTE	g_SrcIP_Addr[] = {192,168,0,29};
//BYTE	g_SrcMAC_Addr[] = {0x00,0x0e,0xa6,0x4b,0xea,0x9a};
BYTE	g_SrcMAC_Addr[] = {0x00,0x53,0x45,0x00,0x00,0x00};
void PrintARP(ARP_PACK *pArp);
void PrintEtherPacket(Frame8019 *Packet);


WORD checksum(WORD *dp, WORD length)
{
	LWORD sum = 0;

	WORD len = length >> 1;
	while(len-- > 0)
		sum += (*dp++);
	if(length & 1)
		sum += (*dp & 0xFF00);
	sum = (sum >> 16) + (sum & 0xFFFF);

	sum += (sum >> 16);
	len = sum & 0xFFFF;
	return(~len);
}


BYTE EtherToArp(Frame8019 *Packet, ARP_PACK	*ArpPacket)
{
	memcpy((void *)ArpPacket, (void *)&Packet->Packet[0], sizeof(ARP_PACK));
	switch (ArpPacket->Operate[1]) {
		case ARP_REQ:			
			return ARP_REQ;
		case ARP_RESP:
			return ARP_RESP;
		default:
			return ARP_NULL;
	}

}

BYTE EtherToIP(Frame8019 *Packet, IP_Header *pIPHeader)
{

	if(!(Packet->Protocol[0] == 0x08 && Packet->Protocol[1] == 0x00))
		return PROTOCOL_IP_NULL;
	
	pIPHeader->Version = Packet->Packet[0]>>4;
	pIPHeader->HeaderLength = (Packet->Packet[0] & 0x0F)<<2;
	pIPHeader->ServiceType = Packet->Packet[1];
	pIPHeader->TotalLength[0] = Packet->Packet[2];
	pIPHeader->TotalLength[1] = Packet->Packet[3];
	pIPHeader->Idenfication[0] = Packet->Packet[4];
	pIPHeader->Idenfication[1] = Packet->Packet[5];
	pIPHeader->DFragment = (Packet->Packet[6] & 0x40)>>6;
	pIPHeader->MFragment = (Packet->Packet[6] & 0x20)>>5;
	pIPHeader->FragmentOffset[0] = Packet->Packet[6] &  0x1F;
	pIPHeader->FragmentOffset[1] = Packet->Packet[7];

	memcpy((void *)(&pIPHeader->LiveTime), (void *)&Packet->Packet[8], 52);
	switch (pIPHeader->Protocol) {
		case 0x01:
			return PROTOCOL_IP_ICMP;
			break;
		case 0x06:
			return PROTOCOL_IP_TCP;
			break;
		case 0x11:
			return PROTOCOL_IP_UDP;
			break;
		default:
			return PROTOCOL_IP_NULL;
			break;
	}

}

BYTE EtherToICMP(Frame8019 *Packet, ICMP_Header *pICMPHeader)
{
	IP_Header IPHeader;
	
	if(EtherToIP(Packet, &IPHeader) != PROTOCOL_IP_ICMP)
		return PROTOCOL_ICMP_NULL;
	memcpy((void *)pICMPHeader, (void *)&Packet->Packet[20], sizeof(ICMP_Header)); 
	switch (pICMPHeader->Type) {
		case 0x08:
			return PROTOCOL_ICMP_REQU;
			break;
		case 0x00:
			return PROTOCOL_ICMP_RESP;
			break;
		default:
			return PROTOCOL_ICMP_NULL;
			break;
	}
	
}

BYTE EtherToUDP(Frame8019 *Packet, UDP_Header *pUDPHeader)
{
	IP_Header IPHeader;
	
	if(EtherToIP(Packet, &IPHeader) != PROTOCOL_IP_UDP)
		return 0;
	memcpy((void *)pUDPHeader, (void *)&Packet->Packet[20], sizeof(UDP_Header)); 
	
	return 1;	
}
BYTE MakeICMPRespond(Frame8019 *Packet, short len)
{
	long i;
	BYTE *pIPData;
	BYTE  temp;
	ICMP_Header icmpHeader;
	
	WORD	 checkSum;


	if(EtherToICMP(Packet, &icmpHeader) != PROTOCOL_ICMP_REQU)
		return 0;

	for(i = 0; i < 6; i++)
	{
		Packet->DestMAC_Addr[i] = Packet->SrcMAC_Addr[i];
		Packet->SrcMAC_Addr[i] = g_SrcMAC_Addr[i];
	}
	
	pIPData = (BYTE *)&Packet->Packet;
	for(i = 0; i < 4; i++)
	{
		pIPData[16+i] = pIPData[12+i];
		pIPData[12+i] = g_SrcIP_Addr[i];
	}
	
	pIPData[0] = 0x45;
/*	pIPData[10] = 0x00;
	pIPData[11] = 0x00;	

	checkSum = checksum((WORD *)(pIPData), 20);
	pIPData[11] = (BYTE)(checkSum >> 8);
	pIPData[10] = (BYTE)(checkSum & 0x00ff);
*/

	pIPData[20] = 0x00;

	pIPData = (BYTE *)&Packet->Packet;
	pIPData[22] = 0x00;	
	pIPData[23] = 0x00;	

	checkSum = checksum((WORD *)(pIPData+20), len - 20);

	pIPData[23] = (BYTE)(checkSum >> 8);
	pIPData[22] = (BYTE)(checkSum & 0x00ff);

	return 1;
}

BYTE SendData(Frame8019 *Packet, Frame8019 *newPacket, BYTE *pBuffer, short len, short nID)
{
	long i;
	BYTE *pIPData;
	BYTE *pUDPData;
	BYTE  temp;
	WORD	 checkSum;
	
	memcpy((void *)newPacket, (void *)Packet, sizeof(Frame8019));

	pIPData = (BYTE *)&newPacket->Packet;


	for(i = 0; i < 6; i++)
	{
		newPacket->DestMAC_Addr[i] = newPacket->SrcMAC_Addr[i];
		newPacket->SrcMAC_Addr[i] = g_SrcMAC_Addr[i];
	}

	for(i = 0; i < 4; i++)
	{
		pIPData[16+i] = pIPData[12+i];
		pIPData[12+i] = g_SrcIP_Addr[i];
	}
	
	pIPData[0] = 0x45;
	pIPData[2] = (len+28+5)>>8;
	pIPData[3] = (len+28+5)&0xff;
	pIPData[10] = 0;
 	pIPData[11] = 0;	
	checkSum = checksum((WORD *)(pIPData), 20);
	pIPData[11] = (BYTE)(checkSum >> 8);
	pIPData[10] = (BYTE)(checkSum & 0xff);	

	temp = pIPData[22];
	pIPData[22] = pIPData[20];
	pIPData[20] = temp;
	temp = pIPData[23];
	pIPData[23] = pIPData[21];
	pIPData[21] = temp;

	pIPData[24] = (len+8+5)>>8;
	pIPData[25] = (len+8+5)&0xff;


	pIPData[26] = 0x00;	
	pIPData[27] = 0x00;	

	pIPData[28] = 'D';
	pIPData[29] = (BYTE)(nID>>8);
	pIPData[30] = (BYTE)(nID & 0x00ff);	
	pIPData[31] = (BYTE)(len>>8);
	pIPData[32] = (BYTE)(len & 0x00ff);
	memcpy((void *)&pIPData[33], (void *)pBuffer, len); 

	return 1;	
}
WORD	MakeArpResponse(ARP_PACK *pArpPack)
{
	short	i;
	
	pArpPack->HardwareType[0] = 0x00;			// 发送者本机网络接口类型（值“1”代表以太网）
	pArpPack->HardwareType[1] = 0x01;
	pArpPack->ProtocolType[0] = 0x08;			// 高级协议:	IP协议 0x0800
	pArpPack->ProtocolType[1] = 0x00;
	pArpPack->HardwareAddrLen = MAC_ADDR_LEN;
	pArpPack->ProtocolAddrLen = IP_ADDR_LEN;
	pArpPack->Operate[0] = 0x00;				// 指出本报文的类型（“1”为ARP请求，“2”为ARP响应)
	pArpPack->Operate[1] = ARP_RESP;
	
	for(i = 0; i < MAC_ADDR_LEN; i++)
	{
		pArpPack->DestMAC_Addr[i] = pArpPack->SrcMAC_Addr[i];	
		pArpPack->SrcMAC_Addr[i] = g_SrcMAC_Addr[i];	
	}

	for(i = 0; i < IP_ADDR_LEN; i++)
	{
		pArpPack->DestIP_Addr[i] = pArpPack->SrcIP_Addr[i];
		pArpPack->SrcIP_Addr[i] = g_SrcIP_Addr[i];
	}
	
	return	sizeof(ARP_PACK);
}



WORD MakeEtherFrame(BYTE *pDestMAC_Addr, BYTE *pProtocol, BYTE *pPacketData, WORD lenth, Frame8019 *pFrame)
{
	short	i;

	for(i = 0; i < MAC_ADDR_LEN; i++)
	{
		pFrame->DestMAC_Addr[i] = pDestMAC_Addr[i];
		pFrame->SrcMAC_Addr[i] = g_SrcMAC_Addr[i];
	}	

	pFrame->Protocol[0] = pProtocol[0];
	pFrame->Protocol[1] = pProtocol[1];
	memcpy((void *)&pFrame->Packet[0], (void *)pPacketData, lenth);

	return (ETHER_FRAME_HEADER_LEN + lenth);
}