/*-----------------------------------------------------------------------------------*/ void icmp_dest_unreach(struct pbuf *p, enum icmp_dur_type t) { struct pbuf *q; struct ip_hdr *iphdr; struct icmp_dur_hdr *idur; q = pbuf_alloc(PBUF_TRANSPORT, 8 + IP_HLEN + 8, PBUF_RAM); /* ICMP header + IP header + 8 bytes of data */ iphdr = p->payload; idur = q->payload; ICMPH_TYPE_SET(idur, ICMP_DUR); ICMPH_CODE_SET(idur, t); bcopy(p->payload, (char *)q->payload + 8, IP_HLEN + 8); /* calculate checksum */ idur->chksum = 0; idur->chksum = inet_chksum(idur, q->len); #ifdef ICMP_STATS ++stats.icmp.xmit; #endif /* ICMP_STATS */ sr_lwip_output(q,&(iphdr->dest), &(iphdr->src), IP_PROTO_ICMP); pbuf_free(q); }
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; }
/*-----------------------------------------------------------------------------------*/ void icmp_time_exceeded(struct pbuf *p, enum icmp_te_type t) { struct pbuf *q; struct ip_hdr *iphdr; struct icmp_te_hdr *tehdr; q = pbuf_alloc(PBUF_TRANSPORT, 8 + IP_HLEN + 8, PBUF_RAM); iphdr = p->payload; #if ICMP_DEBUG DEBUGF(ICMP_DEBUG, ("icmp_time_exceeded from ")); ip_addr_debug_print(&(iphdr->src)); DEBUGF(ICMP_DEBUG, (" to ")); ip_addr_debug_print(&(iphdr->dest)); DEBUGF(ICMP_DEBUG, ("\n")); #endif /* ICMP_DEBNUG */ tehdr = q->payload; ICMPH_TYPE_SET(tehdr, ICMP_TE); ICMPH_CODE_SET(tehdr, t); /* copy fields from original packet */ bcopy((char *)p->payload, (char *)q->payload + 8, IP_HLEN + 8); /* calculate checksum */ tehdr->chksum = 0; tehdr->chksum = inet_chksum(tehdr, q->len); #ifdef ICMP_STATS ++stats.icmp.xmit; #endif /* ICMP_STATS */ sr_lwip_output(q, &(iphdr->dest), &(iphdr->src), IP_PROTO_ICMP); pbuf_free(q); }
void icmp_time_exceeded(struct pbuf *p, enum icmp_te_type t) { struct pbuf *q; struct ip_hdr *iphdr; struct icmp_te_hdr *tehdr; q = pbuf_alloc(PBUF_IP, 8 + IP_HLEN + 8, PBUF_RAM); iphdr = p->payload; LWIP_DEBUGF(ICMP_DEBUG, ("icmp_time_exceeded from ")); ip_addr_debug_print(ICMP_DEBUG, &(iphdr->src)); LWIP_DEBUGF(ICMP_DEBUG, (" to ")); ip_addr_debug_print(ICMP_DEBUG, &(iphdr->dest)); LWIP_DEBUGF(ICMP_DEBUG, ("\n")); tehdr = q->payload; ICMPH_TYPE_SET(tehdr, ICMP_TE); ICMPH_CODE_SET(tehdr, t); /* copy fields from original packet */ memcpy((u8_t *)q->payload + 8, (u8_t *)p->payload, IP_HLEN + 8); /* calculate checksum */ tehdr->chksum = 0; tehdr->chksum = inet_chksum(tehdr, q->len); ICMP_STATS_INC(icmp.xmit); /* increase number of messages attempted to send */ snmp_inc_icmpoutmsgs(); /* increase number of destination unreachable messages attempted to send */ snmp_inc_icmpouttimeexcds(); ip_output(q, NULL, &(iphdr->src), ICMP_TTL, 0, IP_PROTO_ICMP); pbuf_free(q); }
void send_ping(router_t *router, addr_ip_t dest_ip, addr_ip_t src_ip, uint16_t id, uint16_t count) { int len = IPV4_HEADER_LENGTH+8; byte *payload = malloc_or_die(len*sizeof(byte)); //Free'd (below) struct ip_hdr *iphdr = (void *)payload; IPH_VHLTOS_SET(iphdr, 4, 5, 0); IPH_LEN_SET(iphdr, htons(len)); IPH_ID_SET(iphdr, 0); IPH_OFFSET_SET(iphdr, 0); IPH_TTL_SET(iphdr, 64); IPH_PROTO_SET(iphdr, 1); iphdr->src.addr = src_ip; iphdr->dest.addr = dest_ip; IPH_CHKSUM_SET(iphdr, 0); IPH_CHKSUM_SET(iphdr, htons(calc_checksum(payload, IPV4_HEADER_LENGTH))); struct icmp_echo_hdr *pihdr = (void *)payload+IPV4_HEADER_LENGTH; ICMPH_TYPE_SET(pihdr, ICMP_TYPE_ECHO_REQUEST); ICMPH_CODE_SET(pihdr, 0); pihdr->id = id; pihdr->seqno = count; ICMPH_CHKSUM_SET(pihdr, 0); ICMPH_CHKSUM_SET(pihdr, htons(calc_checksum(payload+IPV4_HEADER_LENGTH, 8))); send_packet(payload, src_ip, dest_ip, len, FALSE, FALSE); free(payload); }
void icmp_dest_unreach(struct pbuf *p, enum icmp_dur_type t) { struct pbuf *q; struct ip_hdr *iphdr; struct icmp_dur_hdr *idur; q = pbuf_alloc(PBUF_IP, 8 + IP_HLEN + 8, PBUF_RAM); /* ICMP header + IP header + 8 bytes of data */ iphdr = p->payload; idur = q->payload; ICMPH_TYPE_SET(idur, ICMP_DUR); ICMPH_CODE_SET(idur, t); memcpy((u8_t *)q->payload + 8, p->payload, IP_HLEN + 8); /* calculate checksum */ idur->chksum = 0; idur->chksum = inet_chksum(idur, q->len); ICMP_STATS_INC(icmp.xmit); /* increase number of messages attempted to send */ snmp_inc_icmpoutmsgs(); /* increase number of destination unreachable messages attempted to send */ snmp_inc_icmpoutdestunreachs(); ip_output(q, NULL, &(iphdr->src), ICMP_TTL, 0, IP_PROTO_ICMP); pbuf_free(q); }
/********************************************************************************************************* ** 函数名称: __inetPingPrepare ** 功能描述: 构造 ping 包 ** 输 入 : icmphdrEcho 数据 ** iDataSize 数据大小 ** pusSeqRecv 需要判断的 seq ** 输 出 : NONE ** 全局变量: ** 调用模块: *********************************************************************************************************/ static VOID __inetPingPrepare (struct icmp_echo_hdr *icmphdrEcho, INT iDataSize, UINT16 *pusSeqRecv) { static u16_t usSeqNum = 1; REGISTER INT i; SYS_ARCH_DECL_PROTECT(x); ICMPH_TYPE_SET(icmphdrEcho, ICMP_ECHO); ICMPH_CODE_SET(icmphdrEcho, 0); *pusSeqRecv = usSeqNum; icmphdrEcho->chksum = 0; icmphdrEcho->id = 0xAFAF; /* ID */ icmphdrEcho->seqno = htons(usSeqNum); /* * 填充数据 */ for(i = 0; i < iDataSize; i++) { ((PCHAR)icmphdrEcho)[sizeof(struct icmp_echo_hdr) + i] = (CHAR)(i % 256); } #if CHECKSUM_GEN_ICMP icmphdrEcho->chksum = inet_chksum(icmphdrEcho, (u16_t)(iDataSize + sizeof(struct icmp_echo_hdr))); #endif /* CHECKSUM_GEN_ICMP */ SYS_ARCH_PROTECT(x); usSeqNum++; SYS_ARCH_UNPROTECT(x); }
static void ICACHE_FLASH_ATTR ping_prepare_er(struct icmp_echo_hdr *iecho, u16_t len) { ICMPH_TYPE_SET(iecho, ICMP_ER); ICMPH_CODE_SET(iecho, 0); iecho->chksum = 0; iecho->chksum = inet_chksum(iecho, len); }
/** * Send a 'time exceeded' packet, called from ip_forward() if TTL is 0. * * @param p the input packet for which the 'time exceeded' should be sent, * p->payload pointing to the IP header * @param t type of the 'time exceeded' packet */ void icmp_time_exceeded(struct pbuf *p, enum icmp_te_type t) { struct pbuf *q; struct ip_hdr *iphdr; struct icmp_te_hdr *tehdr; /* ICMP header + IP header + 8 bytes of data */ q = pbuf_alloc(PBUF_IP, sizeof(struct icmp_dur_hdr) + IP_HLEN + ICMP_DEST_UNREACH_DATASIZE, PBUF_RAM); if (q == NULL) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_time_exceeded: failed to allocate pbuf for ICMP packet.\n")); return; } LWIP_ASSERT("check that first pbuf can hold icmp message", (q->len >= (sizeof(struct icmp_dur_hdr) + IP_HLEN + ICMP_DEST_UNREACH_DATASIZE))); iphdr = p->payload; LWIP_DEBUGF(ICMP_DEBUG, ("icmp_time_exceeded from ")); ip_addr_debug_print(ICMP_DEBUG, &(iphdr->src)); LWIP_DEBUGF(ICMP_DEBUG, (" to ")); ip_addr_debug_print(ICMP_DEBUG, &(iphdr->dest)); LWIP_DEBUGF(ICMP_DEBUG, ("\n")); tehdr = q->payload; ICMPH_TYPE_SET(tehdr, ICMP_TE); ICMPH_CODE_SET(tehdr, t); /* copy fields from original packet */ SMEMCPY((u8_t *)q->payload + sizeof(struct icmp_dur_hdr), (u8_t *)p->payload, IP_HLEN + ICMP_DEST_UNREACH_DATASIZE); /* calculate checksum */ tehdr->chksum = 0; tehdr->chksum = inet_chksum(tehdr, q->len); ICMP_STATS_INC(icmp.xmit); /* increase number of messages attempted to send */ snmp_inc_icmpoutmsgs(); /* increase number of destination unreachable messages attempted to send */ snmp_inc_icmpouttimeexcds(); ip_output(q, NULL, &(iphdr->src), ICMP_TTL, 0, IP_PROTO_ICMP); pbuf_free(q); }
/** Prepare a echo ICMP request */ static void ping_prepare_echo(struct icmp_echo_hdr *iecho, struct ping_info_t* ping_info) { int i; ICMPH_TYPE_SET(iecho,ICMP_ECHO); ICMPH_CODE_SET(iecho, 0); iecho->chksum = 0; iecho->id = PING_ID; iecho->seqno = htons(++ping_info->seq_num); iecho->chksum = 0; /* fill the additional data buffer with some data */ for(i = 0; i < ping_info->data_size; i++) { ((char*)iecho)[sizeof(struct icmp_echo_hdr) + i] = i; } iecho->chksum = inet_chksum(iecho, ping_info->size); }
/** Prepare a echo ICMP request */ static void ping_prepare_echo( struct icmp_echo_hdr *iecho, u16_t len) { size_t i; size_t data_len = len - sizeof(struct icmp_echo_hdr); ICMPH_TYPE_SET(iecho, ICMP_ECHO); ICMPH_CODE_SET(iecho, 0); iecho->chksum = 0; iecho->id = PING_ID; iecho->seqno = htons(++ping_seq_num); /* fill the additional data buffer with some data */ for(i = 0; i < data_len; i++) { ((char*)iecho)[sizeof(struct icmp_echo_hdr) + i] = (char)i; } iecho->chksum = inet_chksum(iecho, len); }
static void ping_send(struct raw_pcb *raw, ip_addr_t *addr) { struct pbuf *p; struct icmp_echo_hdr *iecho; p = pbuf_alloc(PBUF_IP,sizeof(struct icmp_echo_hdr),PBUF_RAM); if (!p) return; iecho = p->payload; ICMPH_TYPE_SET(iecho,ICMP_ECHO); iecho->chksum = 0; iecho->seqno = htons(seq_num); iecho->chksum = inet_chksum(iecho, p->len); raw_send_to(raw,p,addr); pbuf_free(p); seq_num++; }
/** * Send an icmp 'destination unreachable' packet, called from ip_input() if * the transport layer protocol is unknown and from udp_input() if the local * port is not bound. * * @param p the input packet for which the 'unreachable' should be sent, * p->payload pointing to the IP header * @param t type of the 'unreachable' packet */ void icmp_dest_unreach(struct pbuf *p, enum icmp_dur_type t) { struct pbuf *q; struct ip_hdr *iphdr; struct icmp_dur_hdr *idur; /* ICMP header + IP header + 8 bytes of data */ q = pbuf_alloc(PBUF_IP, sizeof(struct icmp_dur_hdr) + IP_HLEN + ICMP_DEST_UNREACH_DATASIZE, PBUF_RAM); if (q == NULL) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_dest_unreach: failed to allocate pbuf for ICMP packet.\n")); return; } LWIP_ASSERT("check that first pbuf can hold icmp message", (q->len >= (sizeof(struct icmp_dur_hdr) + IP_HLEN + ICMP_DEST_UNREACH_DATASIZE))); iphdr = p->payload; idur = q->payload; ICMPH_TYPE_SET(idur, ICMP_DUR); ICMPH_CODE_SET(idur, t); SMEMCPY((u8_t *)q->payload + sizeof(struct icmp_dur_hdr), p->payload, IP_HLEN + ICMP_DEST_UNREACH_DATASIZE); /* calculate checksum */ idur->chksum = 0; idur->chksum = inet_chksum(idur, q->len); ICMP_STATS_INC(icmp.xmit); /* increase number of messages attempted to send */ snmp_inc_icmpoutmsgs(); /* increase number of destination unreachable messages attempted to send */ snmp_inc_icmpoutdestunreachs(); ip_output(q, NULL, &(iphdr->src), ICMP_TTL, 0, IP_PROTO_ICMP); pbuf_free(q); }
static void ping_send(int s, ip_addr_t *addr) { struct icmp_echo_hdr *iecho; struct sockaddr_in to; if (!(iecho = (struct icmp_echo_hdr *)malloc(sizeof(struct icmp_echo_hdr)))) return; ICMPH_TYPE_SET(iecho,ICMP_ECHO); iecho->chksum = 0; iecho->seqno = htons(seq_num); iecho->chksum = inet_chksum(iecho, sizeof(*iecho)); to.sin_len = sizeof(to); to.sin_family = AF_INET; to.sin_addr.s_addr = addr->addr; lwip_sendto(s,iecho,sizeof(*iecho),0,(struct sockaddr*)&to,sizeof(to)); free(iecho); seq_num++; }
static void ping_send(int s, const ip_addr_t *addr) { struct icmp_echo_hdr *iecho; struct sockaddr_storage to; if (!(iecho = (struct icmp_echo_hdr *)malloc(sizeof(struct icmp_echo_hdr)))) return; ICMPH_TYPE_SET(iecho,ICMP_ECHO); iecho->chksum = 0; iecho->seqno = htons(seq_num); iecho->chksum = inet_chksum(iecho, sizeof(*iecho)); #if LWIP_IPV4 if(!IP_IS_V6(addr)) { struct sockaddr_in *to4 = (struct sockaddr_in*)&to; to4->sin_len = sizeof(to); to4->sin_family = AF_INET; inet_addr_from_ipaddr(&to4->sin_addr, ip_2_ip4(addr)); } #endif /* LWIP_IPV4 */ #if LWIP_IPV6 if(IP_IS_V6(addr)) { struct sockaddr_in6 *to6 = (struct sockaddr_in6*)&to; to6->sin6_len = sizeof(to); to6->sin6_family = AF_INET6; inet6_addr_from_ip6addr(&to6->sin6_addr, ip_2_ip6(addr)); } #endif /* LWIP_IPV6 */ lwip_sendto(s, iecho, sizeof(*iecho), 0, (struct sockaddr*)&to, sizeof(to)); free(iecho); seq_num++; }
/** * Processes ICMP input packets, called from ip_input(). * * Currently only processes icmp echo requests and sends * out the echo response. * * @param p the icmp echo request packet, p->payload pointing to the icmp header * @param inp the netif on which this packet was received */ void icmp_input(struct pbuf *p, struct netif *inp) { u8_t type; #ifdef LWIP_DEBUG u8_t code; #endif /* LWIP_DEBUG */ struct icmp_echo_hdr *iecho; const struct ip_hdr *iphdr_in; s16_t hlen; const ip4_addr_t* src; ICMP_STATS_INC(icmp.recv); MIB2_STATS_INC(mib2.icmpinmsgs); iphdr_in = ip4_current_header(); hlen = IPH_HL(iphdr_in) * 4; if (hlen < IP_HLEN) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: short IP header (%"S16_F" bytes) received\n", hlen)); goto lenerr; } if (p->len < sizeof(u16_t)*2) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: short ICMP (%"U16_F" bytes) received\n", p->tot_len)); goto lenerr; } type = *((u8_t *)p->payload); #ifdef LWIP_DEBUG code = *(((u8_t *)p->payload)+1); #endif /* LWIP_DEBUG */ switch (type) { case ICMP_ER: /* This is OK, echo reply might have been parsed by a raw PCB (as obviously, an echo request has been sent, too). */ MIB2_STATS_INC(mib2.icmpinechoreps); break; case ICMP_ECHO: MIB2_STATS_INC(mib2.icmpinechos); src = ip4_current_dest_addr(); /* multicast destination address? */ if (ip4_addr_ismulticast(ip4_current_dest_addr())) { #if LWIP_MULTICAST_PING /* For multicast, use address of receiving interface as source address */ src = netif_ip4_addr(inp); #else /* LWIP_MULTICAST_PING */ LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: Not echoing to multicast pings\n")); goto icmperr; #endif /* LWIP_MULTICAST_PING */ } /* broadcast destination address? */ if (ip4_addr_isbroadcast(ip4_current_dest_addr(), ip_current_netif())) { #if LWIP_BROADCAST_PING /* For broadcast, use address of receiving interface as source address */ src = netif_ip4_addr(inp); #else /* LWIP_BROADCAST_PING */ LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: Not echoing to broadcast pings\n")); goto icmperr; #endif /* LWIP_BROADCAST_PING */ } LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ping\n")); if (p->tot_len < sizeof(struct icmp_echo_hdr)) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: bad ICMP echo received\n")); goto lenerr; } #if CHECKSUM_CHECK_ICMP IF__NETIF_CHECKSUM_ENABLED(inp, NETIF_CHECKSUM_CHECK_ICMP) { if (inet_chksum_pbuf(p) != 0) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: checksum failed for received ICMP echo\n")); pbuf_free(p); ICMP_STATS_INC(icmp.chkerr); MIB2_STATS_INC(mib2.icmpinerrors); return; } } #endif #if LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN if (pbuf_header(p, (hlen + PBUF_LINK_HLEN + PBUF_LINK_ENCAPSULATION_HLEN))) { /* p is not big enough to contain link headers * allocate a new one and copy p into it */ struct pbuf *r; /* allocate new packet buffer with space for link headers */ r = pbuf_alloc(PBUF_LINK, p->tot_len + hlen, PBUF_RAM); if (r == NULL) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: allocating new pbuf failed\n")); goto icmperr; } if (r->len < hlen + sizeof(struct icmp_echo_hdr)) { LWIP_DEBUGF(ICMP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("first pbuf cannot hold the ICMP header")); pbuf_free(r); goto icmperr; } /* copy the ip header */ MEMCPY(r->payload, iphdr_in, hlen); /* switch r->payload back to icmp header (cannot fail) */ if (pbuf_header(r, -hlen)) { LWIP_ASSERT("icmp_input: moving r->payload to icmp header failed\n", 0); pbuf_free(r); goto icmperr; } /* copy the rest of the packet without ip header */ if (pbuf_copy(r, p) != ERR_OK) { LWIP_DEBUGF(ICMP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("icmp_input: copying to new pbuf failed")); pbuf_free(r); goto icmperr; } /* free the original p */ pbuf_free(p); /* we now have an identical copy of p that has room for link headers */ p = r; } else { /* restore p->payload to point to icmp header (cannot fail) */ if (pbuf_header(p, -(s16_t)(hlen + PBUF_LINK_HLEN + PBUF_LINK_ENCAPSULATION_HLEN))) { LWIP_ASSERT("icmp_input: restoring original p->payload failed\n", 0); goto icmperr; } } #endif /* LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN */ /* At this point, all checks are OK. */ /* We generate an answer by switching the dest and src ip addresses, * setting the icmp type to ECHO_RESPONSE and updating the checksum. */ iecho = (struct icmp_echo_hdr *)p->payload; if (pbuf_header(p, hlen)) { LWIP_DEBUGF(ICMP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("Can't move over header in packet")); } else { err_t ret; struct ip_hdr *iphdr = (struct ip_hdr*)p->payload; ip4_addr_copy(iphdr->src, *src); ip4_addr_copy(iphdr->dest, *ip4_current_src_addr()); ICMPH_TYPE_SET(iecho, ICMP_ER); #if CHECKSUM_GEN_ICMP IF__NETIF_CHECKSUM_ENABLED(inp, NETIF_CHECKSUM_GEN_ICMP) { /* adjust the checksum */ if (iecho->chksum > PP_HTONS(0xffffU - (ICMP_ECHO << 8))) { iecho->chksum += PP_HTONS(ICMP_ECHO << 8) + 1; } else { iecho->chksum += PP_HTONS(ICMP_ECHO << 8); } } #if LWIP_CHECKSUM_CTRL_PER_NETIF else { iecho->chksum = 0; } #endif /* LWIP_CHECKSUM_CTRL_PER_NETIF */ #else /* CHECKSUM_GEN_ICMP */ iecho->chksum = 0; #endif /* CHECKSUM_GEN_ICMP */ /* Set the correct TTL and recalculate the header checksum. */ IPH_TTL_SET(iphdr, ICMP_TTL); IPH_CHKSUM_SET(iphdr, 0); #if CHECKSUM_GEN_IP IF__NETIF_CHECKSUM_ENABLED(inp, NETIF_CHECKSUM_GEN_IP) { IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, hlen)); } #endif /* CHECKSUM_GEN_IP */ ICMP_STATS_INC(icmp.xmit); /* increase number of messages attempted to send */ MIB2_STATS_INC(mib2.icmpoutmsgs); /* increase number of echo replies attempted to send */ MIB2_STATS_INC(mib2.icmpoutechoreps); /* send an ICMP packet */ ret = ip4_output_if(p, src, IP_HDRINCL, ICMP_TTL, 0, IP_PROTO_ICMP, inp); if (ret != ERR_OK) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ip_output_if returned an error: %s\n", lwip_strerr(ret))); } }
/** * Processes ICMP input packets, called from ip_input(). * * Currently only processes icmp echo requests and sends * out the echo response. * * @param p the icmp echo request packet, p->payload pointing to the icmp header * @param inp the netif on which this packet was received */ void icmp_input(struct pbuf *p, struct netif *inp) { u8_t type; #ifdef LWIP_DEBUG u8_t code; #endif /* LWIP_DEBUG */ struct icmp_echo_hdr *iecho; struct ip_hdr *iphdr; s16_t hlen; ICMP_STATS_INC(icmp.recv); snmp_inc_icmpinmsgs(); iphdr = (struct ip_hdr *)ip_current_header(); hlen = IPH_HL(iphdr) * 4; if (p->len < sizeof(u16_t)*2) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: short ICMP (%"U16_F" bytes) received\n", p->tot_len)); goto lenerr; } type = *((u8_t *)p->payload); #ifdef LWIP_DEBUG code = *(((u8_t *)p->payload)+1); #endif /* LWIP_DEBUG */ switch (type) { case ICMP_ER: /* This is OK, echo reply might have been parsed by a raw PCB (as obviously, an echo request has been sent, too). */ break; case ICMP_ECHO: #if !LWIP_MULTICAST_PING || !LWIP_BROADCAST_PING { int accepted = 1; #if !LWIP_MULTICAST_PING /* multicast destination address? */ if (ip_addr_ismulticast(ip_current_dest_addr())) { accepted = 0; } #endif /* LWIP_MULTICAST_PING */ #if !LWIP_BROADCAST_PING /* broadcast destination address? */ if (ip_addr_isbroadcast(ip_current_dest_addr(), inp)) { accepted = 0; } #endif /* LWIP_BROADCAST_PING */ /* broadcast or multicast destination address not acceptd? */ if (!accepted) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: Not echoing to multicast or broadcast pings\n")); ICMP_STATS_INC(icmp.err); pbuf_free(p); return; } } #endif /* !LWIP_MULTICAST_PING || !LWIP_BROADCAST_PING */ LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ping\n")); if (p->tot_len < sizeof(struct icmp_echo_hdr)) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: bad ICMP echo received\n")); goto lenerr; } #if CHECKSUM_CHECK_ICMP if (inet_chksum_pbuf(p) != 0) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: checksum failed for received ICMP echo\n")); pbuf_free(p); ICMP_STATS_INC(icmp.chkerr); snmp_inc_icmpinerrors(); return; } #endif #if LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN if (pbuf_header(p, (PBUF_IP_HLEN + PBUF_LINK_HLEN))) { /* p is not big enough to contain link headers * allocate a new one and copy p into it */ struct pbuf *r; /* switch p->payload to ip header */ if (pbuf_header(p, hlen)) { LWIP_ASSERT("icmp_input: moving p->payload to ip header failed\n", 0); goto memerr; } /* allocate new packet buffer with space for link headers */ r = pbuf_alloc(PBUF_LINK, p->tot_len, PBUF_RAM); if (r == NULL) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: allocating new pbuf failed\n")); goto memerr; } LWIP_ASSERT("check that first pbuf can hold struct the ICMP header", (r->len >= hlen + sizeof(struct icmp_echo_hdr))); /* copy the whole packet including ip header */ if (pbuf_copy(r, p) != ERR_OK) { LWIP_ASSERT("icmp_input: copying to new pbuf failed\n", 0); goto memerr; } iphdr = (struct ip_hdr *)r->payload; /* switch r->payload back to icmp header */ if (pbuf_header(r, -hlen)) { LWIP_ASSERT("icmp_input: restoring original p->payload failed\n", 0); goto memerr; } /* free the original p */ pbuf_free(p); /* we now have an identical copy of p that has room for link headers */ p = r; } else { /* restore p->payload to point to icmp header */ if (pbuf_header(p, -(s16_t)(PBUF_IP_HLEN + PBUF_LINK_HLEN))) { LWIP_ASSERT("icmp_input: restoring original p->payload failed\n", 0); goto memerr; } } #endif /* LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN */ /* At this point, all checks are OK. */ /* We generate an answer by switching the dest and src ip addresses, * setting the icmp type to ECHO_RESPONSE and updating the checksum. */ iecho = (struct icmp_echo_hdr *)p->payload; ip_addr_copy(iphdr->src, *ip_current_dest_addr()); ip_addr_copy(iphdr->dest, *ip_current_src_addr()); ICMPH_TYPE_SET(iecho, ICMP_ER); #if CHECKSUM_GEN_ICMP /* adjust the checksum */ if (iecho->chksum > PP_HTONS(0xffffU - (ICMP_ECHO << 8))) { iecho->chksum += PP_HTONS(ICMP_ECHO << 8) + 1; } else { iecho->chksum += PP_HTONS(ICMP_ECHO << 8); } #else /* CHECKSUM_GEN_ICMP */ iecho->chksum = 0; #endif /* CHECKSUM_GEN_ICMP */ /* Set the correct TTL and recalculate the header checksum. */ IPH_TTL_SET(iphdr, ICMP_TTL); IPH_CHKSUM_SET(iphdr, 0); #if CHECKSUM_GEN_IP IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, IP_HLEN)); #endif /* CHECKSUM_GEN_IP */ ICMP_STATS_INC(icmp.xmit); /* increase number of messages attempted to send */ snmp_inc_icmpoutmsgs(); /* increase number of echo replies attempted to send */ snmp_inc_icmpoutechoreps(); if(pbuf_header(p, hlen)) { LWIP_ASSERT("Can't move over header in packet", 0); } else { err_t ret; /* send an ICMP packet, src addr is the dest addr of the curren packet */ ret = ip_output_if(p, ip_current_dest_addr(), IP_HDRINCL, ICMP_TTL, 0, IP_PROTO_ICMP, inp); if (ret != ERR_OK) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ip_output_if returned an error: %c.\n", ret)); } } break; default: LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ICMP type %"S16_F" code %"S16_F" not supported.\n", (s16_t)type, (s16_t)code)); ICMP_STATS_INC(icmp.proterr); ICMP_STATS_INC(icmp.drop); } pbuf_free(p); return; lenerr: pbuf_free(p); ICMP_STATS_INC(icmp.lenerr); snmp_inc_icmpinerrors(); return; #if LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN memerr: pbuf_free(p); ICMP_STATS_INC(icmp.err); snmp_inc_icmpinerrors(); return; #endif /* LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN */ }
void icmp_input(struct pbuf *p, struct netif *inp) { u8_t type; u8_t code; struct icmp_echo_hdr *iecho; struct ip_hdr *iphdr; struct ip_addr tmpaddr; u16_t hlen; u8_t *payload = NULL; ICMP_STATS_INC(icmp.recv); snmp_inc_icmpinmsgs(); iphdr = p->payload; hlen = IPH_HL(iphdr) * 4; if (p->tot_len < sizeof(u16_t)*2) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: short ICMP (%"U16_F" bytes) received\n", p->tot_len)); pbuf_free(p); ICMP_STATS_INC(icmp.lenerr); snmp_inc_icmpinerrors(); return; } payload = (u8_t *)p->payload + hlen; type = *payload; code = *(payload+1); switch (type) { case ICMP_ECHO: /* broadcast or multicast destination address? */ if (ip_addr_isbroadcast(&iphdr->dest, inp) || ip_addr_ismulticast(&iphdr->dest)) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: Not echoing to multicast or broadcast pings\n")); ICMP_STATS_INC(icmp.err); pbuf_free(p); return; } LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ping\n")); if (p->tot_len < sizeof(struct icmp_echo_hdr)) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: bad ICMP echo received\n")); pbuf_free(p); ICMP_STATS_INC(icmp.lenerr); snmp_inc_icmpinerrors(); return; } iecho = (struct icmp_echo_hdr *)payload; if (inet_chksum_pbuf(p) != 0) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: checksum failed for received ICMP echo\n")); pbuf_free(p); ICMP_STATS_INC(icmp.chkerr); snmp_inc_icmpinerrors(); return; } tmpaddr.addr = iphdr->src.addr; iphdr->src.addr = iphdr->dest.addr; iphdr->dest.addr = tmpaddr.addr; ICMPH_TYPE_SET(iecho, ICMP_ER); /* adjust the checksum */ if (iecho->chksum >= htons(0xffff - (ICMP_ECHO << 8))) { iecho->chksum += htons(ICMP_ECHO << 8) + 1; } else { iecho->chksum += htons(ICMP_ECHO << 8); } ICMP_STATS_INC(icmp.xmit); /* increase number of messages attempted to send */ snmp_inc_icmpoutmsgs(); /* increase number of echo replies attempted to send */ snmp_inc_icmpoutechoreps(); pbuf_header(p, hlen); ip_output_if(p, &(iphdr->src), IP_HDRINCL, IPH_TTL(iphdr), 0, IP_PROTO_ICMP, inp); break; default: LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ICMP type %"S16_F" code %"S16_F" not supported.\n", (s16_t)type, (s16_t)code)); ICMP_STATS_INC(icmp.proterr); ICMP_STATS_INC(icmp.drop); } pbuf_free(p); }
/** * Processes ICMP input packets, called from ip_input(). * * Currently only processes icmp echo requests and sends * out the echo response. * * @param p the icmp echo request packet, p->payload pointing to the ip header * @param inp the netif on which this packet was received */ void icmp_input(struct pbuf *p, struct netif *inp) { u8_t type; #ifdef LWIP_DEBUG u8_t code; #endif /* LWIP_DEBUG */ struct icmp_echo_hdr *iecho; struct ip_hdr *iphdr; struct ip_addr tmpaddr; s16_t hlen; u8_t iptxt[20]; volatile u8_t iptab[4]; u32_t IPaddress; ICMP_STATS_INC(icmp.recv); snmp_inc_icmpinmsgs(); iphdr = p->payload; hlen = IPH_HL(iphdr) * 4; if (pbuf_header(p, -hlen) || (p->tot_len < sizeof(u16_t)*2)) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: short ICMP (%"U16_F" bytes) received\n", p->tot_len)); goto lenerr; } IPaddress = iphdr->src.addr; /* read its IP address */ iptab[0] = (u8_t)(IPaddress >> 24); iptab[1] = (u8_t)(IPaddress >> 16); iptab[2] = (u8_t)(IPaddress >> 8); iptab[3] = (u8_t)(IPaddress); sprintf((char*)iptxt, "Ping: %d.%d.%d.%d ", iptab[3], iptab[2], iptab[1], iptab[0]); LCD_DisplayStringLine(Line4, iptxt); printf("\n\r%s", iptxt); type = *((u8_t *)p->payload); #ifdef LWIP_DEBUG code = *(((u8_t *)p->payload)+1); #endif /* LWIP_DEBUG */ switch (type) { case ICMP_ECHO: #if !LWIP_MULTICAST_PING || !LWIP_BROADCAST_PING { int accepted = 1; #if !LWIP_MULTICAST_PING /* multicast destination address? */ if (ip_addr_ismulticast(&iphdr->dest)) { accepted = 0; } #endif /* LWIP_MULTICAST_PING */ #if !LWIP_BROADCAST_PING /* broadcast destination address? */ if (ip_addr_isbroadcast(&iphdr->dest, inp)) { accepted = 0; } #endif /* LWIP_BROADCAST_PING */ /* broadcast or multicast destination address not acceptd? */ if (!accepted) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: Not echoing to multicast or broadcast pings\n")); ICMP_STATS_INC(icmp.err); pbuf_free(p); return; } } #endif /* !LWIP_MULTICAST_PING || !LWIP_BROADCAST_PING */ LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ping\n")); if (p->tot_len < sizeof(struct icmp_echo_hdr)) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: bad ICMP echo received\n")); goto lenerr; } if (inet_chksum_pbuf(p) != 0) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: checksum failed for received ICMP echo\n")); pbuf_free(p); ICMP_STATS_INC(icmp.chkerr); snmp_inc_icmpinerrors(); return; } #if LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN if (pbuf_header(p, (PBUF_IP_HLEN + PBUF_LINK_HLEN))) { /* p is not big enough to contain link headers * allocate a new one and copy p into it */ struct pbuf *r; /* switch p->payload to ip header */ if (pbuf_header(p, hlen)) { LWIP_ASSERT("icmp_input: moving p->payload to ip header failed\n", 0); goto memerr; } /* allocate new packet buffer with space for link headers */ r = pbuf_alloc(PBUF_LINK, p->tot_len, PBUF_RAM); if (r == NULL) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: allocating new pbuf failed\n")); goto memerr; } LWIP_ASSERT("check that first pbuf can hold struct the ICMP header", (r->len >= hlen + sizeof(struct icmp_echo_hdr))); /* copy the whole packet including ip header */ if (pbuf_copy(r, p) != ERR_OK) { LWIP_ASSERT("icmp_input: copying to new pbuf failed\n", 0); goto memerr; } iphdr = r->payload; /* switch r->payload back to icmp header */ if (pbuf_header(r, -hlen)) { LWIP_ASSERT("icmp_input: restoring original p->payload failed\n", 0); goto memerr; } /* free the original p */ pbuf_free(p); /* we now have an identical copy of p that has room for link headers */ p = r; } else { /* restore p->payload to point to icmp header */ if (pbuf_header(p, -(s16_t)(PBUF_IP_HLEN + PBUF_LINK_HLEN))) { LWIP_ASSERT("icmp_input: restoring original p->payload failed\n", 0); goto memerr; } } #endif /* LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN */ /* At this point, all checks are OK. */ /* We generate an answer by switching the dest and src ip addresses, * setting the icmp type to ECHO_RESPONSE and updating the checksum. */ iecho = p->payload; tmpaddr.addr = iphdr->src.addr; iphdr->src.addr = iphdr->dest.addr; iphdr->dest.addr = tmpaddr.addr; ICMPH_TYPE_SET(iecho, ICMP_ER); /* This part of code has been modified by ST's MCD Application Team */ /* To use the Checksum Offload Engine for the putgoing ICMP packets, the ICMP checksum field should be set to 0, this is required only for Tx ICMP*/ #ifdef CHECKSUM_BY_HARDWARE iecho->chksum = 0; #else /* adjust the checksum */ if (iecho->chksum >= htons(0xffff - (ICMP_ECHO << 8))) { iecho->chksum += htons(ICMP_ECHO << 8) + 1; } else { iecho->chksum += htons(ICMP_ECHO << 8); } #endif /* Set the correct TTL and recalculate the header checksum. */ IPH_TTL_SET(iphdr, ICMP_TTL); IPH_CHKSUM_SET(iphdr, 0); #if CHECKSUM_GEN_IP IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, IP_HLEN)); #endif /* CHECKSUM_GEN_IP */ ICMP_STATS_INC(icmp.xmit); /* increase number of messages attempted to send */ snmp_inc_icmpoutmsgs(); /* increase number of echo replies attempted to send */ snmp_inc_icmpoutechoreps(); if(pbuf_header(p, hlen)) { LWIP_ASSERT("Can't move over header in packet", 0); } else { err_t ret; ret = ip_output_if(p, &(iphdr->src), IP_HDRINCL, ICMP_TTL, 0, IP_PROTO_ICMP, inp); if (ret != ERR_OK) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ip_output_if returned an error: %c.\n", ret)); } } break; default: LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ICMP type %"S16_F" code %"S16_F" not supported.\n", (s16_t)type, (s16_t)code)); ICMP_STATS_INC(icmp.proterr); ICMP_STATS_INC(icmp.drop); } pbuf_free(p); return; lenerr: pbuf_free(p); ICMP_STATS_INC(icmp.lenerr); snmp_inc_icmpinerrors(); return; #if LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN memerr: pbuf_free(p); ICMP_STATS_INC(icmp.err); snmp_inc_icmpinerrors(); return; #endif /* LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN */ }
/** * Processes ICMP input packets, called from ip_input(). * * Currently only processes icmp echo requests and sends * out the echo response. * * @param p the icmp echo request packet, p->payload pointing to the ip header * @param inp the netif on which this packet was received */ void icmp_input(PBUF *p, NETIF *inp) { uint8 type; ICMP_ECHO_HDR *iecho; IP_HDR *iphdr; IP_ADDR tmpaddr; int16 hlen; iphdr = p->payload; hlen = IPH_HL(iphdr) * 4; if (pbuf_header(p, -hlen) == 0) return; type = *((uint8 *)p->payload); iecho = p->payload; printf("icmp_input %d %d\n", iecho->type, iecho->code); switch (type) { case ICMP_ECHO: { int accepted = 1; /* multicast destination address? */ if (ip_addr_ismulticast(&iphdr->dest)) { accepted = 0; } /* broadcast destination address? */ if (ip_addr_isbroadcast(&iphdr->dest, inp)) { accepted = 0; } /* broadcast or multicast destination address not acceptd? */ if (!accepted) { pbuf_free(p); return; } if (p->len < sizeof(ICMP_ECHO_HDR)) goto lenerr; if (inet_chksum_pbuf(p) != 0) { pbuf_free(p); return; } /* At this point, all checks are OK. */ /* We generate an answer by switching the dest and src ip addresses, * setting the icmp type to ECHO_RESPONSE and updating the checksum. */ tmpaddr.addr = iphdr->src.addr; iphdr->src.addr = iphdr->dest.addr; iphdr->dest.addr = tmpaddr.addr; iecho = p->payload; ICMPH_TYPE_SET(iecho, ICMP_ER); /* adjust the checksum */ if (iecho->chksum >= htons(0xffff - (ICMP_ECHO << 8))) { iecho->chksum += htons(ICMP_ECHO << 8) + 1; } else { iecho->chksum += htons(ICMP_ECHO << 8); } if (pbuf_header(p, hlen) == 0) return; ip_output_if(p, &(iphdr->src), IP_HDRINCL, ICMP_TTL, 0, IP_PROTO_ICMP, inp); break; } default: break; } lenerr: pbuf_free(p); return; }
/*-----------------------------------------------------------------------------------*/ void icmp_input(struct pbuf *p, struct netif *inp) { unsigned char type; struct icmp_echo_hdr *iecho; struct ip_hdr *iphdr; struct ip_addr tmpaddr; uint16_t hlen; #ifdef ICMP_STATS ++stats.icmp.recv; #endif /* ICMP_STATS */ iphdr = p->payload; hlen = IPH_HL(iphdr) * 4/sizeof(uint8_t); pbuf_header(p, -hlen); type = *((uint8_t *)p->payload); switch(type) { case ICMP_ECHO: if(ip_addr_isbroadcast(&iphdr->dest, &inp->netmask) || ip_addr_ismulticast(&iphdr->dest)) { DEBUGF(ICMP_DEBUG, ("Smurf.\n")); #ifdef ICMP_STATS ++stats.icmp.err; #endif /* ICMP_STATS */ pbuf_free(p); return; } DEBUGF(ICMP_DEBUG, ("icmp_input: ping\n")); DEBUGF(DEMO_DEBUG, ("Pong!\n")); if(p->tot_len < sizeof(struct icmp_echo_hdr)) { DEBUGF(ICMP_DEBUG, ("icmp_input: bad ICMP echo received\n")); pbuf_free(p); #ifdef ICMP_STATS ++stats.icmp.lenerr; #endif /* ICMP_STATS */ return; } iecho = p->payload; if(inet_chksum_pbuf(p) != 0) { DEBUGF(ICMP_DEBUG, ("icmp_input: checksum failed for received ICMP echo\n")); pbuf_free(p); #ifdef ICMP_STATS ++stats.icmp.chkerr; #endif /* ICMP_STATS */ return; } tmpaddr.addr = iphdr->src.addr; iphdr->src.addr = iphdr->dest.addr; iphdr->dest.addr = tmpaddr.addr; ICMPH_TYPE_SET(iecho, ICMP_ER); /* adjust the checksum */ if(iecho->chksum >= htons(0xffff - (ICMP_ECHO << 8))) { iecho->chksum += htons(ICMP_ECHO << 8) + 1; } else { iecho->chksum += htons(ICMP_ECHO << 8); } #ifdef ICMP_STATS ++stats.icmp.xmit; #endif /* ICMP_STATS */ pbuf_header(p, hlen); /* XXX .mc */ assert(0); /*sr_lwip_output(p, IP_HDRINCL, IP_PROTO_ICMP);*/ break; default: DEBUGF(ICMP_DEBUG, ("icmp_input: ICMP type not supported.\n")); #ifdef ICMP_STATS ++stats.icmp.proterr; ++stats.icmp.drop; #endif /* ICMP_STATS */ } pbuf_free(p); }
bool generate_response_ICMP_packet(packet_info_t *pi, int type, int code) { byte *old_packet = malloc_or_die(pi->len-IPV4_HEADER_OFFSET); //Free'd (below). memcpy(old_packet, pi->packet+IPV4_HEADER_OFFSET, pi->len-IPV4_HEADER_OFFSET); struct ip_hdr *old_iphdr = (void *)old_packet; free(pi->packet); pi->len = IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH+36;//14 for Ethernet header, 20 for IPv4 header and 36 for ICMP time exceeded packet. pi->packet = malloc_or_die((pi->len)*sizeof(byte)); //Free'd (before). struct eth_hdr *ethhdr = (void *)pi->packet; ETH_DEST_SET(ethhdr, make_mac_addr(0, 0, 0, 0, 0, 0)); ETH_SRC_SET(ethhdr, make_mac_addr(0, 0, 0, 0, 0, 0)); ETH_TYPE_SET(ethhdr, 0); struct ip_hdr *iphdr = (void *)pi->packet+IPV4_HEADER_OFFSET; IPH_VHLTOS_SET(iphdr, 4, 5, 0); //Set version and IHL. IPH_LEN_SET(iphdr, htons(pi->len-IPV4_HEADER_OFFSET)); IPH_ID_SET(iphdr, 0); IPH_OFFSET_SET(iphdr, 0); IPH_TTL_SET(iphdr, 32); IPH_PROTO_SET(iphdr, 1); addr_ip_t dest = IPH_SRC(old_iphdr); addr_ip_t target = sr_integ_findnextip(dest); interface_t *source_intf = sr_integ_findsrcintf(target); if (source_intf == NULL) { char ip_str[STRLEN_IP], target_str[STRLEN_IP]; ip_to_string(ip_str, dest); ip_to_string(target_str, target); debug_println("No route to source %s with target %s, dropping packet!", ip_str, target_str); free(old_packet); return 1; } addr_ip_t source = source_intf->ip; char dest_str[15]; ip_to_string(dest_str, dest); char source_str[15]; ip_to_string(source_str, source); debug_println("dest=%s, source=%s", dest_str, source_str); memcpy(&IPH_SRC(iphdr), &source, 6); memcpy(&IPH_DEST(iphdr), &dest, 6); byte *icmp_packet = pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH; struct icmp_dur_hdr *icmp_hdr = (void *)pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH; ICMPH_TYPE_SET(icmp_hdr, type); ICMPH_CODE_SET(icmp_hdr, code); ICMPH_UNUSED_SET(icmp_hdr); memcpy(icmp_packet+ICMP_TIME_EX_IP_HEADER_OFFSET, old_packet, IPV4_HEADER_LENGTH + 8); ICMPH_CHKSUM_SET(icmp_hdr, 0); ICMPH_CHKSUM_SET(icmp_hdr, htons(calc_checksum(icmp_packet, pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH))); free(old_packet); return 0; }
/* 调用途径:ethernetif_input() -> ethernet_input() -> ip_input() -> icmp_input()*/ void icmp_input(struct pbuf *p, struct netif *inp) { u8_t type; #ifdef LWIP_DEBUG u8_t code; #endif /* LWIP_DEBUG */ struct icmp_echo_hdr *iecho; struct ip_hdr *iphdr; /* IP 地址 */ struct ip_addr tmpaddr; s16_t hlen; ICMP_STATS_INC(icmp.recv); snmp_inc_icmpinmsgs(); /* 指向IP首部 */ iphdr = p->payload; /* 获得IP报头长度 */ hlen = IPH_HL(iphdr) * 4; /* pbuf的payload向后移动到IP载荷,即ICMP报头处 * 如果IP的载荷小于4字节,则跳到lenerr处,释放pbuf */ if (pbuf_header(p, -hlen) || (p->tot_len < sizeof(u16_t)*2)) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: short ICMP (%"U16_F" bytes) received\n", p->tot_len)); goto lenerr; } /* 获取ICMP报头中的类型 */ type = *((u8_t *)p->payload); #ifdef LWIP_DEBUG code = *(((u8_t *)p->payload)+1); #endif /* LWIP_DEBUG */ switch (type) { /* 如果ICMP类型是回显请求 */ case ICMP_ECHO: /* 先检查目的IP地址是否合法 */ #if !LWIP_MULTICAST_PING || !LWIP_BROADCAST_PING { /* accepted表示是否对ICMP回显请求进行回应 */ int accepted = 1; #if !LWIP_MULTICAST_PING /* multicast destination address? */ /* 如果目的IP地址是多播地址,则不回应 */ if (ip_addr_ismulticast(&iphdr->dest)) { accepted = 0; } #endif /* LWIP_MULTICAST_PING */ #if !LWIP_BROADCAST_PING /* broadcast destination address? */ /* 如果目的IP地址是广播地址,则不回应 */ if (ip_addr_isbroadcast(&iphdr->dest, inp)) { accepted = 0; } #endif /* LWIP_BROADCAST_PING */ /* broadcast or multicast destination address not acceptd? */ /* 如果不回应,则释放pbuf后,返回 */ if (!accepted) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: Not echoing to multicast or broadcast pings\n")); ICMP_STATS_INC(icmp.err); pbuf_free(p); return; } } #endif /* !LWIP_MULTICAST_PING || !LWIP_BROADCAST_PING */ LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ping\n")); /* 检查ICMP报文长度是否合法,ICMP报文总长度不能小于ICMP报头长度8字节 */ if (p->tot_len < sizeof(struct icmp_echo_hdr)) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: bad ICMP echo received\n")); /* 跳到lenerr处执行返回操作 */ goto lenerr; } /* 计算ICMP的校验和是否正确 */ if (inet_chksum_pbuf(p) != 0) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: checksum failed for received ICMP echo\n")); /* ICMP校验和错误,则释放pbuf,并返回 */ pbuf_free(p); ICMP_STATS_INC(icmp.chkerr); snmp_inc_icmpinerrors(); return; } #if LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN if (pbuf_header(p, (PBUF_IP_HLEN + PBUF_LINK_HLEN))) { /* p is not big enough to contain link headers * allocate a new one and copy p into it */ struct pbuf *r; /* switch p->payload to ip header */ if (pbuf_header(p, hlen)) { LWIP_ASSERT("icmp_input: moving p->payload to ip header failed\n", 0); goto memerr; } /* allocate new packet buffer with space for link headers */ r = pbuf_alloc(PBUF_LINK, p->tot_len, PBUF_RAM); if (r == NULL) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: allocating new pbuf failed\n")); goto memerr; } LWIP_ASSERT("check that first pbuf can hold struct the ICMP header", (r->len >= hlen + sizeof(struct icmp_echo_hdr))); /* copy the whole packet including ip header */ if (pbuf_copy(r, p) != ERR_OK) { LWIP_ASSERT("icmp_input: copying to new pbuf failed\n", 0); goto memerr; } iphdr = r->payload; /* switch r->payload back to icmp header */ if (pbuf_header(r, -hlen)) { LWIP_ASSERT("icmp_input: restoring original p->payload failed\n", 0); goto memerr; } /* free the original p */ pbuf_free(p); /* we now have an identical copy of p that has room for link headers */ p = r; } else { /* restore p->payload to point to icmp header */ if (pbuf_header(p, -(s16_t)(PBUF_IP_HLEN + PBUF_LINK_HLEN))) { LWIP_ASSERT("icmp_input: restoring original p->payload failed\n", 0); goto memerr; } } #endif /* LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN */ /* At this point, all checks are OK. */ /* We generate an answer by switching the dest and src ip addresses, * setting the icmp type to ECHO_RESPONSE and updating the checksum. */ /* 校验完成,调整ICMP回显请求的相关字段,生成回显应答 * 交换IP数据报的源IP和目的IP地址,填写ICMP报文的类型字段,并重新计算ICMP的校验和 */ /* 获取ICMP报头指针 */ iecho = p->payload; /* 交换IP报头的源IP地址和目的IP地址 */ tmpaddr.addr = iphdr->src.addr; iphdr->src.addr = iphdr->dest.addr; iphdr->dest.addr = tmpaddr.addr; /* 设置ICMP报文类型为回显应答 */ ICMPH_TYPE_SET(iecho, ICMP_ER); /* adjust the checksum */ /* 调整ICMP的校验和 */ if (iecho->chksum >= htons(0xffff - (ICMP_ECHO << 8))) { iecho->chksum += htons(ICMP_ECHO << 8) + 1; } else { iecho->chksum += htons(ICMP_ECHO << 8); } /* Set the correct TTL and recalculate the header checksum. */ /* 设置IP首部的TTL */ IPH_TTL_SET(iphdr, ICMP_TTL); /* 计算IP首部校验和 */ IPH_CHKSUM_SET(iphdr, 0); #if CHECKSUM_GEN_IP IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, IP_HLEN)); #endif /* CHECKSUM_GEN_IP */ /* 注意:这里没有修改IP首部的标识字段,所以ICMP回显应答的IP首部的标识字段和 * ICMP回显请求的标识字段是相同的。理论上来说ICMP回显应答的IP首部标识字段应该 * 被修改,但为什么不修改? * 对Linux主机进行ping,Linux主机回复的ICMP回显应答的IP首部的标识字段就修改了 */ ICMP_STATS_INC(icmp.xmit); /* increase number of messages attempted to send */ snmp_inc_icmpoutmsgs(); /* increase number of echo replies attempted to send */ snmp_inc_icmpoutechoreps(); /* pbuf的payload由ICMP的报头移动到IP的报头,hlen保存了IP报头的长度 */ if(pbuf_header(p, hlen)) { LWIP_ASSERT("Can't move over header in packet", 0); /* 移动失败 */ } else { /* 移动成功 */ err_t ret; /* 调用ip_output_if发送IP数据报,IP_HDRINCL表示IP首部已经填写好,并且 * pbuf的payload指向IP数据报首部,而不是IP载荷首部 */ ret = ip_output_if(p, &(iphdr->src), IP_HDRINCL, ICMP_TTL, 0, IP_PROTO_ICMP, inp); if (ret != ERR_OK) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ip_output_if returned an error: %c.\n", ret)); } } break; /* 如果ICMP类型不是ICMP回显请求,则直接忽略 */ default: LWIP_DEBUGF(ICMP_DEBUG, ("icmp_input: ICMP type %"S16_F" code %"S16_F" not supported.\n", (s16_t)type, (s16_t)code)); /* 更新统计量 */ ICMP_STATS_INC(icmp.proterr); ICMP_STATS_INC(icmp.drop); } /* 释放pbuf */ pbuf_free(p); return; lenerr: pbuf_free(p); ICMP_STATS_INC(icmp.lenerr); snmp_inc_icmpinerrors(); return; #if LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN memerr: pbuf_free(p); ICMP_STATS_INC(icmp.err); snmp_inc_icmpinerrors(); return; #endif /* LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN */ }
int netstack_send_echo(u8 *ripaddr, u16 size, u16 seqno, struct netstack_echo_reply *reply) { u64 ts; int s, i, err; char buf[64]; size_t fromlen, off, len = sizeof(struct icmp_echo_hdr) + size; ip_addr_t to_addr, from_addr; struct sockaddr_in sock; struct ip_hdr *iphdr; struct icmp_echo_hdr *iecho; LWIP_ASSERT("ping_size is too big\n", len <= 0xffff); /* Prepare target address */ IP4_ADDR(&to_addr, ripaddr[0],ripaddr[1],ripaddr[2],ripaddr[3]); /* Open RAW socket */ if ((s = lwip_socket(AF_INET, SOCK_RAW, IP_PROTO_ICMP)) < 0) { vmm_printf("%s: failed to open ICMP socket\n", __func__); return VMM_EFAIL; } /* Set socket option */ i = PING_RCV_TIMEO; lwip_setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, &i, sizeof(i)); /* Prepare socket address */ sock.sin_len = sizeof(sock); sock.sin_family = AF_INET; inet_addr_from_ipaddr(&sock.sin_addr, &to_addr); /* Prepare ECHO request */ iecho = (struct icmp_echo_hdr *)vmm_zalloc(len); if (!iecho) { return VMM_ENOMEM; } ICMPH_TYPE_SET(iecho, ICMP_ECHO); ICMPH_CODE_SET(iecho, 0); iecho->chksum = 0; iecho->id = PING_ID; iecho->seqno = htons(seqno); for (i = 0; i < size; i++) { ((char*)iecho)[sizeof(struct icmp_echo_hdr) + i] = (char)i; } iecho->chksum = inet_chksum(iecho, len); /* Send ECHO request */ err = lwip_sendto(s, iecho, len, 0, (struct sockaddr*)&sock, sizeof(sock)); vmm_free(iecho); if (!err) { return VMM_EFAIL; } /* Get reference timestamp */ ts = vmm_timer_timestamp(); /* Wait for ECHO reply */ err = VMM_EFAIL; off = lwip_recvfrom(s, buf, sizeof(buf), 0, (struct sockaddr*)&sock, (socklen_t*)&fromlen); if (off >= (sizeof(struct ip_hdr) + sizeof(struct icmp_echo_hdr))) { inet_addr_to_ipaddr(&from_addr, &sock.sin_addr); iphdr = (struct ip_hdr *)buf; iecho = (struct icmp_echo_hdr *)(buf + (IPH_HL(iphdr) * 4)); if ((iecho->id == PING_ID) && (iecho->seqno == htons(seqno))) { reply->ripaddr[0] = ip4_addr1(&from_addr); reply->ripaddr[1] = ip4_addr2(&from_addr); reply->ripaddr[2] = ip4_addr3(&from_addr); reply->ripaddr[3] = ip4_addr4(&from_addr); reply->ttl = IPH_TTL(iphdr); reply->len = len; reply->seqno = seqno; reply->rtt = udiv64(vmm_timer_timestamp() - ts, 1000); err = VMM_OK; } } while (off < len) { off = lwip_recvfrom(s, buf, sizeof(buf), 0, (struct sockaddr*)&sock, (socklen_t*)&fromlen); } /* Close RAW socket */ lwip_close(s); return err; }
int netstack_send_echo(u8 *ripaddr, u16 size, u16 seqno, struct netstack_echo_reply *reply) { int i, rc; u64 timeout = PING_DELAY_NS; struct pbuf *p; struct icmp_echo_hdr *iecho; size_t len = sizeof(struct icmp_echo_hdr) + size; LWIP_ASSERT("ping_size <= 0xffff", len <= 0xffff); /* Lock ping context for atomicity */ vmm_mutex_lock(&lns.ping_lock); /* Alloc ping pbuf */ p = pbuf_alloc(PBUF_IP, (u16_t)len, PBUF_RAM); if (!p) { vmm_mutex_unlock(&lns.ping_lock); return VMM_ENOMEM; } if ((p->len != p->tot_len) || (p->next != NULL)) { pbuf_free(p); vmm_mutex_unlock(&lns.ping_lock); return VMM_EFAIL; } /* Prepare ECHO request */ iecho = (struct icmp_echo_hdr *)p->payload; ICMPH_TYPE_SET(iecho, ICMP_ECHO); ICMPH_CODE_SET(iecho, 0); iecho->chksum = 0; iecho->id = PING_ID; iecho->seqno = htons(seqno); for (i = 0; i < size; i++) { ((char*)iecho)[sizeof(struct icmp_echo_hdr) + i] = (char)i; } iecho->chksum = inet_chksum(iecho, len); /* Prepare target address */ IP4_ADDR(&lns.ping_addr, ripaddr[0],ripaddr[1],ripaddr[2],ripaddr[3]); /* Save ping info */ lns.ping_seq_num = seqno; lns.ping_reply = reply; lns.ping_recv_tstamp = 0; lns.ping_send_tstamp = vmm_timer_timestamp(); lns.ping_recv_tstamp = lns.ping_send_tstamp + PING_DELAY_NS; /* Send ping packet */ raw_sendto(lns.ping_pcb, p, &lns.ping_addr); /* Wait for ping to complete with timeout */ timeout = lns.ping_recv_tstamp - lns.ping_send_tstamp; rc = vmm_completion_wait_timeout(&lns.ping_done, &timeout); timeout = lns.ping_recv_tstamp - lns.ping_send_tstamp; lns.ping_reply->rtt = udiv64(timeout, 1000); /* Free ping pbuf */ pbuf_free(p); /* Clear ping reply pointer */ lns.ping_reply = NULL; /* Unloack ping context */ vmm_mutex_unlock(&lns.ping_lock); return rc; }