pingtest.c

STR71X系列ARM微控制器原理与实践配套光盘

/*****************************************************************************\
*                                                                             *
*  以太网控制器测试模块                                                       *
*                                                                             *
*  该模块实现了基于以太网的ARP回应和ICMP回应                                  *
*                                                                             *
\*****************************************************************************/

#include "PingTest.h"
#include "Ethernet.h"

// 本地IP地址
// TODO: 可根据需要改为其它地址
IP_ADDR g_LocalIPAddr = 192u << 24 | 168u << 16 | 196u << 8 | 200u;

// 以太网协议头
typedef struct eth_hdr {
	u16 padding;
	ETH_ADDR dest, src;
	u16 type;
} ETH_HDR;

#define ETHTYPE_ARP		0x0806
#define ETHTYPE_IP		0x0800

// ARP协议头
typedef struct arp_hdr {
	u16 hwtype;
	u16 proto;
	u16 _hwlen_protolen;
	u16 opcode;
	ETH_ADDR shwaddr;
	u16 sip_h, sip_l;
	ETH_ADDR dhwaddr;
	u16 dip_h, dip_l;
} ARP_HDR;

#define ARP_REQUEST		1
#define ARP_REPLY		2

// IP协议头
typedef struct ip_hdr {
	u16 _v_hl_tos;		// version / header length / type of service
	u16 len;				// total length
	u16 id;				// identification
	u16 offset;			// fragment offset field
#define IP_RF		0x8000	// reserved fragment flag
#define IP_DF		0x4000	// dont fragment flag
#define IP_MF		0x2000	// more fragments flag
#define IP_OFFMASK	0x1fff	// mask for fragmenting bits
	u8 ttl, proto;		// time to live / protocol
	u16 chksum;			// checksum
	IP_ADDR src, dest;		// source and destination IP addresses
} IP_HDR;

#define IPH_V(hdr) (ntohs((hdr)->_v_hl_tos) >> 12)
#define IPH_HL(hdr) ((ntohs((hdr)->_v_hl_tos) >> 6) & 0x3c)

#define IP_PROTO_ICMP 1

// ICMP协议头
typedef struct icmp_hdr {
	u8 type, code;
	u16 chksum;
	u16 id;
	u16 seqno;
} ICMP_HDR;

#define ICMP_ER		0
#define ICMP_ECHO	8


// 交换字节顺序（双字节）
__inline u16 bswap16(u16 x)
{
	return x << 8 | x >> 8;
}

// 交换字节顺序（四字节）
__inline u32 bswap32(u32 x)
{
	return x << 24 | x << 8 & 0x00ff0000
		| x >> 8 & 0x0000ff00 | x >> 24;
}

#define htons bswap16
#define htonl bswap32
#define ntohs htons
#define ntohl htonl

// 计算校验和
static u32 inet_chksum(void *dataptr, int len)
{
	u32 acc;

	for(acc = 0; len > 1; len -= 2)
	{
		acc += *(u16 *)dataptr;
		dataptr = (u16 *)dataptr + 1;
	}

	if(len)
		acc += htons((*(u8 *)dataptr) << 8);

	while(acc >> 16)
		acc = (acc & 0xffff) + (acc >> 16);

	return acc ^ 0xffff;
}

// 回应IP包
static void ip_reply(IP_HDR *iphdr, int len)
{
	ETH_HDR *ethhdr = (ETH_HDR *)iphdr - 1;
	int i;

	iphdr->dest = iphdr->src;
	iphdr->src = htonl(g_LocalIPAddr);

	iphdr->len = htons(len);
	iphdr->id = 0;
	iphdr->offset = htons(IP_DF);

	iphdr->chksum = 0;
	iphdr->chksum = inet_chksum(iphdr, sizeof(IP_HDR));

	for(i = 0; i < 3; i++)
	{
		ethhdr->dest[i] = ethhdr->src[i];
		ethhdr->src[i] = local_eth_addr[i];
	}
	NIC_SendPack((u16 *)ethhdr + 1, len + (sizeof(ETH_HDR) - 2));
}

// 处理ICMP输入
static void icmp_input(IP_HDR *iphdr, int len)
{
	ICMP_HDR *icmphdr;

	if(len < sizeof(IP_HDR) + sizeof(ICMP_HDR))
		return;

	icmphdr = (ICMP_HDR *)(iphdr + 1);
	if(icmphdr->type != ICMP_ECHO)
		return;
	if(inet_chksum(icmphdr, len - sizeof(IP_HDR)))
		return;

	icmphdr->type = ICMP_ER;
	if(icmphdr->chksum >= htons(0xffff - (ICMP_ECHO << 8)))
		icmphdr->chksum += htons(ICMP_ECHO << 8) + 1;
	else
		icmphdr->chksum += htons(ICMP_ECHO << 8);
	ip_reply(iphdr, len);
}

// 处理IP输入
static void ip_input(IP_HDR *iphdr, int len)
{
	if(IPH_V(iphdr) != 4)
		return;
	if(IPH_HL(iphdr) != sizeof(IP_HDR))
		return;
	if(inet_chksum(iphdr, sizeof(IP_HDR)))
		return;
	if(iphdr->offset & htons(IP_OFFMASK | IP_MF))
		return;

	switch(iphdr->proto)
	{
	case IP_PROTO_ICMP:
		icmp_input(iphdr, len);
		break;
	}
}

// 处理ARP输入
static void arp_input(ETH_HDR *ethhdr, int len)
{
	int i;
	ARP_HDR *arphdr;

	if(len < sizeof(ETH_HDR) + sizeof(ARP_HDR))
		return;

	arphdr = (ARP_HDR *)(ethhdr + 1);
	if((ntohs(arphdr->dip_h) << 16 | ntohs(arphdr->dip_l)) != g_LocalIPAddr)
		return;

	switch(ntohs(arphdr->opcode))
	{
	case ARP_REQUEST:
		arphdr->opcode = htons(ARP_REPLY);
		arphdr->dip_h = arphdr->sip_h;
		arphdr->dip_l = arphdr->sip_l;
		arphdr->sip_h = htons(g_LocalIPAddr >> 16);
		arphdr->sip_l = htons(g_LocalIPAddr & 0xffff);
		for(i = 0; i < 3; i++)
		{
			ethhdr->dest[i] = arphdr->dhwaddr[i] = arphdr->shwaddr[i];
			ethhdr->src[i] = arphdr->shwaddr[i] = local_eth_addr[i];
		}
		NIC_SendPack((u16 *)ethhdr + 1, len - 2);
		break;
	}
}

// 处理以太网输入
static void eth_input(ETH_HDR *ethhdr, int len)
{
	switch(htons(ethhdr->type))
	{
	case ETHTYPE_ARP:
		arp_input(ethhdr, len);
		break;
	case ETHTYPE_IP:
		ip_input((IP_HDR *)(ethhdr + 1), len - sizeof(ETH_HDR));
		break;
	}
}

///////////////////////////////////////////////////////////////////////////////
//
//	功能：
//			以太网测试入口函数
//	参数：
//			buf			以太网数据包地址，必须按4字节对齐，必须在开头包含2字节填充
//			len			数据包长度（包括2字节填充）
//	返回值：
//			无
//
void PingTest_Input(void *buf, int len)
{
	eth_input((ETH_HDR *)buf, len);
}