C++ ICMPH_TYPE_SET函数代码示例

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