自学教程：C++ ICMPH_TYPE_SET函数代码示例

/*-----------------------------------------------------------------------------------*/voidicmp_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;}

/*-----------------------------------------------------------------------------------*/voidicmp_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);}

voidicmp_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);}

voidicmp_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_ATTRping_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 */voidicmp_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 voidping_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 voidping_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 */voidicmp_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 voidping_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++;}

//.........这里部分代码省略.........        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)));      }    }

//.........这里部分代码省略.........      /* 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_LENmemerr:  pbuf_free(p);  ICMP_STATS_INC(icmp.err);  snmp_inc_icmpinerrors();  return;#endif /* LWIP_ICMP_ECHO_CHECK_INPUT_PBUF_LEN */}

voidicmp_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);}

//.........这里部分代码省略.........      /* 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_LENmemerr:  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;}

/*-----------------------------------------------------------------------------------*/voidicmp_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;}

//.........这里部分代码省略.........        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);			/* 移动失败 */

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