libping initial commitHEADmaster

1 files changed, 311 insertions, 0 deletions

diff --git a/libping.c b/libping.c
new file mode 100644
index 0000000..b65b802
--- /dev/null
+++ b/libping.c
@@ -0,0 +1,311 @@
+#include "libping.h"
+
+/* From Stevens, UNP2ev1 */
+unsigned short
+in_cksum(unsigned short *addr, int len)
+{
+	int nleft = len;
+	int sum = 0;
+	unsigned short *w = addr;
+	unsigned short answer = 0;
+
+	while (nleft > 1) 
+	{
+		sum += *w++;
+		nleft -= 2;
+	}
+
+	if (nleft == 1) 
+	{
+		*(unsigned char *)(&answer) = *(unsigned char *)w;
+		sum += answer;
+	}
+
+	sum = (sum >> 16) + (sum & 0xffff);
+	sum += (sum >> 16);
+	answer = ~sum;
+	return (answer);
+}
+
+// Computing the internet checksum (RFC 1071).
+// Note that the internet checksum does not preclude collisions.
+uint16_t
+ip_checksum (uint16_t *addr, int len)
+{
+  int count = len;
+  register uint32_t sum = 0;
+  uint16_t answer = 0;
+
+  // Sum up 2-byte values until none or only one byte left.
+  while (count > 1) {
+    sum += *(addr++);
+    count -= 2;
+  }
+
+  // Add left-over byte, if any.
+  if (count > 0) {
+    sum += *(uint8_t *) addr;
+  }
+
+  // Fold 32-bit sum into 16 bits; we lose information by doing this,
+  // increasing the chances of a collision.
+  // sum = (lower 16 bits) + (upper 16 bits shifted right 16 bits)
+  while (sum >> 16) {
+    sum = (sum & 0xffff) + (sum >> 16);
+  }
+
+  // Checksum is one's compliment of sum.
+  answer = ~sum;
+
+  return (answer);
+}
+
+
+int icmp_eth_init( icmp_pckt_t *icmp )
+{
+	uint8_t dest[] = {0x00,0x22,0x43,0x68,0xe5,0xab};
+	uint8_t src[] = {0x6c,0x88,0x14,0x0a,0x89,0x58};
+
+	memcpy( icmp->eth_dest, dest, 6 );
+	memcpy( icmp->eth_src,  src,  6 );
+	icmp->eth_type = 0x0008;
+	return 0;
+}
+
+
+int icmp_eth_dest( icmp_pckt_t *icmp, uint8_t *mac )
+{
+	memcpy( icmp->eth_dest, mac, 6 );
+	return 0;
+}
+
+
+int icmp_eth_src( icmp_pckt_t *icmp, uint8_t *mac )
+{
+	memcpy( icmp->eth_src, mac, 6 );
+	return 0;
+}
+
+int icmp_eth_len( icmp_pckt_t *icmp )
+{
+	return 14;
+}
+
+
+int icmp_ip_init( icmp_pckt_t *icmp )
+{
+	icmp->head_hlength = 0x45; //ip4,header size 20 byte
+	icmp->tos = 0x00;
+	icmp->ip_length = htons(20+8+ICMP_MAX_DATA); //ip4 header + icmp message
+	icmp->ip_ident = htons(0xfbea);
+	icmp->flags_fragment_off = 0x0040;
+	icmp->ttl = 64;
+	icmp->protocol = 1; //ICMP
+	icmp->source_addr = inet_addr("127.0.0.1");
+	icmp->dest_addr = inet_addr("127.0.0.1");
+	return 0;
+}
+
+
+int icmp_ip_chksum( icmp_pckt_t *icmp )
+{
+	uint16_t chk;
+
+	icmp->ip_chksum = 0;
+	chk = ip_checksum( (uint16_t *)icmp+7, 20 );
+
+	icmp->ip_chksum = chk;
+	return 0;
+}
+
+
+int icmp_ip_dest( icmp_pckt_t *icmp, uint32_t dest )
+{
+	uint32_t s = dest;
+	uint8_t a,b,c,d;
+
+	a = s;
+	b = s>>8;
+	c = s>>16;
+	d = s>>24;
+
+	uint32_t aa = a;
+	uint32_t bb = b;
+	uint32_t cc = c;
+	uint32_t dd = d;
+
+	printf("%d.%d.%d.%d\n",a,b,c,d);
+
+	s = 0x0;
+	s = aa + (bb<<8) + (cc<<16) + (dd<<24);
+	icmp->dest_addr = s;
+	return 0;
+}
+
+
+int icmp_ip_src( icmp_pckt_t *icmp, uint32_t src )
+{
+	uint32_t s = src;
+	uint8_t a,b,c,d;
+
+	a = s;
+	b = s>>8;
+	c = s>>16;
+	d = s>>24;
+
+	uint32_t aa = a;
+	uint32_t bb = b;
+	uint32_t cc = c;
+	uint32_t dd = d;
+
+	printf("%d.%d.%d.%d\n",a,b,c,d);
+
+	s = 0x0;
+	s = aa + (bb<<8) + (cc<<16) + (dd<<24);
+
+	icmp->source_addr = s;
+	return 0;
+}
+
+int icmp_ip_ident( icmp_pckt_t *icmp, uint16_t ident )
+{
+	icmp->ip_ident = htons(ident);
+	//icmp->ip_ident = ident;
+	return 0;
+}
+
+
+int icmp_ip_ttl( icmp_pckt_t *icmp, uint8_t ttl )
+{
+	icmp->ttl = ttl;
+	return 0;
+}
+
+
+int icmp_ip_len( icmp_pckt_t *icmp )
+{
+	return 20;
+}
+
+
+int icmp_icmp_init( icmp_pckt_t *icmp )
+{
+	int i = 0;
+
+	icmp->type = ICMP_TREQUEST;
+	icmp->code = 0;
+	icmp->icmp_ident = 0x4062;
+	icmp->seq_num = 0x0001;
+	//icmp->timestamp = 0x5749aae7;//0xe7aa4957;
+	
+	for (i=0;i<ICMP_MAX_DATA;i++)
+	{
+		icmp->data[i] = 0;
+	}
+	
+	return 0;
+}
+
+
+int icmp_icmp_set_chks( icmp_pckt_t *icmp )
+{
+	uint16_t chk=0x0;
+	uint16_t offset=0x0;
+
+	icmp->icmp_check_sum = 0x0000;
+
+	//fot ICMP header
+	offset = icmp_eth_len(icmp) + icmp_ip_len(icmp);
+	printf("offset %d len %d\n", offset, icmp_icmp_len(icmp));
+	chk = ip_checksum( ((uint16_t *)icmp)+offset/2,
+		icmp_icmp_len(icmp) );
+
+	icmp->icmp_check_sum = chk;
+	return 0;
+}
+
+
+int icmp_icmp_set_code( icmp_pckt_t *icmp, uint8_t code )
+{
+	return 0;
+}
+
+
+int icmp_icmp_set_type( icmp_pckt_t *icmp, uint8_t type )
+{
+	icmp->type = type;
+	return 0;
+}
+
+
+int icmp_icmp_len( icmp_pckt_t *icmp )
+{
+	return 8+ICMP_MAX_DATA;
+}
+
+int icmp_icmp_ident( icmp_pckt_t *icmp, uint16_t ident )
+{
+	icmp->icmp_ident = htons(ident);
+	return 0;
+}
+
+int icmp_icmp_seq( icmp_pckt_t *icmp, uint16_t seq)
+{
+	icmp->seq_num = htons(seq);
+	return 0;
+}
+
+int icmp_total_len( icmp_pckt_t *icmp )
+{
+	int ret=0;
+
+	ret = icmp_eth_len(icmp)
+		+icmp_ip_len(icmp)
+		+icmp_icmp_len(icmp);
+
+	return ret;
+}
+
+int icmp_icmp_ts( icmp_pckt_t *icmp, uint64_t timestamp )
+{
+	//icmp->timestamp = timestamp;
+	return 0;
+}
+
+int icmp_dump( icmp_pckt_t *icmp )
+{
+	int i=0,j=0,m=0;
+
+	printf("Etherenet header\n");
+	for (i=0;i<j+14;i++)
+	{
+		printf("%02X ", ((unsigned char *)icmp)[i]);
+		if (i%16==15) printf("\n");
+	}
+	printf("\n");
+
+	j = i;
+	printf("IP header\n");
+	for (i=j,m=0;i<j+20;i++,m++)
+	{
+		printf("%02X ", ((unsigned char *)icmp)[i]);
+		if (m%16==15) printf("\n");
+	}
+	printf("\n");
+
+	j = i;
+	printf("ICMP header\n");
+	for (i=j,m=0;i<sizeof(icmp_pckt_t);i++,m++)
+	{
+		printf("%02X ", ((unsigned char *)icmp)[i]);
+		if (m%16==15) printf("\n");
+	}
+	printf("\n");
+	return 0;
+}
+
+int icmp_data( icmp_pckt_t *icmp, uint8_t *data, size_t size )
+{
+	memcpy( icmp->data, data, size );
+	return 0;
+}
\ No newline at end of file