path: root/libping.c

#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;
}