bool handle_ICMP_packet(packet_info_t *pi) {
debug_println("Packet is ICMP");
byte *icmp_packet = pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH;
struct icmp_echo_hdr *icmp_hdr = (void *)pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH;
/*unsigned i;
for (i = 0; i < pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH; i++)
printf("(%d %02X) ", i, (int)*(icmp_packet+i));
printf("\n");*/
if (calc_checksum(icmp_packet, pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH)) {
debug_println("ICMP checksum failed, dropping packet!");
return 1;
}
uint8_t type = ICMPH_TYPE(icmp_hdr);
switch (type) {
case ICMP_TYPE_ECHO_REQUEST: ICMPH_TYPE_SET(icmp_hdr, 0);
break;
case ICMP_TYPE_ECHO_REPLY: handle_ping_reply(pi); return 1;
default: debug_println("Dropping packet: type %d unknown.", type); return 1;
}
ICMPH_CHKSUM_SET(icmp_hdr, 0);
ICMPH_CHKSUM_SET(icmp_hdr, htons(calc_checksum(icmp_packet, pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH)));
/*for (i = 0; i < pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH; i++)
printf("(%d %02X) ", i, (int)*(icmp_packet+i));
printf("\n");*/
return 0;
}
static void
ping_recv(int s)
{
char buf[64];
int fromlen, len;
struct sockaddr_in from;
struct ip_hdr *iphdr;
struct icmp_echo_hdr *iecho;
fromlen = sizeof(from);
while((len = lwip_recvfrom(s, buf, sizeof(buf), 0, (struct sockaddr*)&from, (socklen_t*)&fromlen)) > 0) {
if (len >= (int)(sizeof(struct ip_hdr)+sizeof(struct icmp_echo_hdr))) {
ip_addr_t fromaddr;
inet_addr_to_ipaddr(&fromaddr, &from.sin_addr);
LWIP_DEBUGF( PING_DEBUG, ("ping: recv "));
ip_addr_debug_print(PING_DEBUG, &fromaddr);
LWIP_DEBUGF( PING_DEBUG, (" %"U32_F" ms\r\n", (sys_now() - ping_time)));
iphdr = (struct ip_hdr *)buf;
iecho = (struct icmp_echo_hdr *)(buf + (IPH_HL(iphdr) * 4));
if ((iecho->id == PING_ID) && (iecho->seqno == htons(ping_seq_num))) {
/* do some ping result processing */
PING_RESULT((ICMPH_TYPE(iecho) == ICMP_ER));
return;
} else {
LWIP_DEBUGF( PING_DEBUG, ("ping: drop\r\n"));
}
}
}
if (len == 0) {
LWIP_DEBUGF( PING_DEBUG, ("ping: recv - %"U32_F" ms - timeout\r\n", (sys_now()-ping_time)));
}
/* do some ping result processing */
PING_RESULT(0);
}
