static struct netif *
ip_forward(struct pbuf *p, struct ip_hdr *iphdr, struct netif *inp)
{
struct netif *netif;
PERF_START;
/* Find network interface where to forward this IP packet to. */
netif = ip_route((struct ip_addr *)&(iphdr->dest));
if (netif == NULL) {
LWIP_DEBUGF(IP_DEBUG, ("ip_forward: no forwarding route for 0x%"X32_F" found\n",
iphdr->dest.addr));
snmp_inc_ipoutnoroutes();
return (struct netif *)NULL;
}
/* Do not forward packets onto the same network interface on which
* they arrived. */
if (netif == inp) {
LWIP_DEBUGF(IP_DEBUG, ("ip_forward: not bouncing packets back on incoming interface.\n"));
snmp_inc_ipoutnoroutes();
return (struct netif *)NULL;
}
/* decrement TTL */
IPH_TTL_SET(iphdr, IPH_TTL(iphdr) - 1);
/* send ICMP if TTL == 0 */
if (IPH_TTL(iphdr) == 0) {
snmp_inc_ipinhdrerrors();
/* Don't send ICMP messages in response to ICMP messages */
if (IPH_PROTO(iphdr) != IP_PROTO_ICMP) {
icmp_time_exceeded(p, ICMP_TE_TTL);
}
return (struct netif *)NULL;
}
/* Incrementally update the IP checksum. */
if (IPH_CHKSUM(iphdr) >= htons(0xffff - 0x100)) {
IPH_CHKSUM_SET(iphdr, IPH_CHKSUM(iphdr) + htons(0x100) + 1);
} else {
IPH_CHKSUM_SET(iphdr, IPH_CHKSUM(iphdr) + htons(0x100));
}
LWIP_DEBUGF(IP_DEBUG, ("ip_forward: forwarding packet to 0x%"X32_F"\n",
iphdr->dest.addr));
IP_STATS_INC(ip.fw);
IP_STATS_INC(ip.xmit);
snmp_inc_ipforwdatagrams();
PERF_STOP("ip_forward");
/* transmit pbuf on chosen interface */
netif->output(netif, p, (struct ip_addr *)&(iphdr->dest));
return netif;
}
// remove MTD_FLASHMEM to speedup routing
err_t MTD_FLASHMEM Router::netif_input(pbuf *p, netif *inp) {
eth_hdr *ethdr = (eth_hdr *)p->payload;
if (ntohs(ethdr->type) == ETHTYPE_IP) {
// move buffer pointer to start of IP header
pbuf_header(p, -sizeof(eth_hdr));
ip_hdr *iphdr = (ip_hdr *)(p->payload);
// needs to route?
// 1. check match of source interface IP/netmask and destination IP
bool route = ((iphdr->dest.addr & inp->netmask.addr) != (inp->ip_addr.addr & inp->netmask.addr));
// 2. check if not multicast or broadcast (>=224.0.0.0 up to 255.255.255.255)
route = route && ((iphdr->dest.addr & 0xE0) != 0xE0);
if (route) {
/*
debug("netif_input intf=%d len=%d id=%d prot=%d src=%s dst=%s route?=%c\r\n",
inp->num, p->tot_len, IPH_ID(iphdr), IPH_PROTO(iphdr),
(char const*)IPAddress(iphdr->src.addr).get_str(),
(char const*)IPAddress(iphdr->dest.addr).get_str(),
route?'Y':'N');
*/
// find destination interface
ip_addr_t ipdest;
ipdest.addr = iphdr->dest.addr;
netif *destIntf = ip_route(&ipdest);
// decrement TTL
IPH_TTL_SET(iphdr, IPH_TTL(iphdr) - 1);
if (IPH_TTL(iphdr) > 0) {
// update IP checksum
if (IPH_CHKSUM(iphdr) >= PP_HTONS(0xffffU - 0x100))
IPH_CHKSUM_SET(iphdr, IPH_CHKSUM(iphdr) + PP_HTONS(0x100) + 1);
else
IPH_CHKSUM_SET(iphdr, IPH_CHKSUM(iphdr) + PP_HTONS(0x100));
// send the packet
ip_output_if(p, NULL, IP_HDRINCL, 0, 0, 0, destIntf);
}
pbuf_free(p);
return ERR_OK;
}
// restore buffer pointer to start of Ethernet header
pbuf_header(p, +sizeof(eth_hdr));
}
return (Router::s_prevInput[inp->num])(p, inp);
}
/*-----------------------------------------------------------------------------------*/
static void
ip_forward(struct pbuf *p, struct ip_hdr *iphdr, struct netif *inp)
{
static struct netif *netif;
PERF_START;
if((netif = ip_route((struct ip_addr *)&(iphdr->dest))) == NULL) {
DEBUGF(IP_DEBUG, ("ip_forward: no forwarding route for 0x%lx found\n",
iphdr->dest.addr));
return;
}
/* Don't forward packets onto the same network interface on which
they arrived. */
if(netif == inp) {
DEBUGF(IP_DEBUG, ("ip_forward: not forward packets back on incoming interface.\n"));
return;
}
/* Decrement TTL and send ICMP if ttl == 0. */
IPH_TTL_SET(iphdr, IPH_TTL(iphdr) - 1);
if(IPH_TTL(iphdr) == 0) {
/* Don't send ICMP messages in response to ICMP messages */
if(IPH_PROTO(iphdr) != IP_PROTO_ICMP) {
icmp_time_exceeded(p, ICMP_TE_TTL);
}
return;
}
/* Incremental update of the IP checksum. */
if(IPH_CHKSUM(iphdr) >= htons(0xffff - 0x100)) {
IPH_CHKSUM_SET(iphdr, IPH_CHKSUM(iphdr) + htons(0x100) + 1);
} else {
IPH_CHKSUM_SET(iphdr, IPH_CHKSUM(iphdr) + htons(0x100));
}
DEBUGF(IP_DEBUG, ("ip_forward: forwarding packet to 0x%lx\n",
iphdr->dest.addr));
#ifdef IP_STATS
++stats.ip.fw;
++stats.ip.xmit;
#endif /* IP_STATS */
PERF_STOP("ip_forward");
netif->output(netif, p, (struct ip_addr *)&(iphdr->dest));
}
/* Ping using the raw ip */
static u8_t ping_recv(void *arg, struct raw_pcb *pcb, struct pbuf *p,
struct ip_addr *addr)
{
struct icmp_echo_hdr *iecho;
struct ip_hdr *ip = p->payload;
struct ping_info_t* ping_info = (struct ping_info_t*) arg;
uint32_t us;
#if ( (LWIP_VERSION) != ((1U << 24) | (3U << 16) | (2U << 8) | (LWIP_VERSION_RC)) )
struct ip_addr ip_adr;
/* to get unpacked version without losing alignment */
memcpy(&ip_adr, &(ip->src), sizeof(ip_adr));
#endif
if (pbuf_header( p, -PBUF_IP_HLEN)==0) {
iecho = p->payload;
if ((iecho->id == PING_ID) &&
(iecho->seqno == htons(ping_info->seq_num))) {
ping_info->last_rx_tm = timer_get_ms();
ping_info->num_rx++;
us = 1000 *
(ping_info->last_rx_tm - ping_info->last_tx_tm);
if (!ping_info->quiet)
#if ( (LWIP_VERSION) == ((1U << 24) | (3U << 16) | (2U << 8) | (LWIP_VERSION_RC)) )
printk("%d bytes from %s: icmp_seq=%d ttl=%d " \
"time=%d.%03d ms\n",
p->tot_len, ip2str(ip->src),
ntohs(iecho->seqno),
IPH_TTL(ip),
us / 1000, us % 1000);
#else
printk("%d bytes from %s: icmp_seq=%d ttl=%d " \
"time=%d.%03d ms\n",
p->tot_len, ip2str(ip_adr),
ntohs(iecho->seqno),
IPH_TTL(ip),
us / 1000, us % 1000);
#endif
/* do some ping result processing */
ping_info->flags |= PING_REPLY;
}
}
pbuf_free(p);
return 1; /* eat the event */
}
/*********************************************************************************************************
** 函数名称: __inetPingRecv
** 功能描述: 接收 ping 包
** 输 入 : iSock socket
** usSeqRecv 需要判断的 seq
** piTTL 接收到的 TTL
** 输 出 : ERROR
** 全局变量:
** 调用模块:
*********************************************************************************************************/
static INT __inetPingRecv (INT iSock, UINT16 usSeqRecv, INT *piTTL)
{
CHAR cBuffer[512];
INT iCnt = 20; /* 默认最多接收的数据包数 */
REGISTER ssize_t sstLen;
INT iAddLen = sizeof(struct sockaddr_in);
struct sockaddr_in sockaddrinFrom;
struct ip_hdr *iphdrFrom;
struct icmp_echo_hdr *icmphdrFrom;
while ((sstLen = recvfrom(iSock, cBuffer, sizeof(cBuffer), 0,
(struct sockaddr *)&sockaddrinFrom, (socklen_t *)&iAddLen)) > 0) {
if (sstLen >= (sizeof(struct ip_hdr) + sizeof(struct icmp_echo_hdr))) {
iphdrFrom = (struct ip_hdr *)cBuffer;
icmphdrFrom = (struct icmp_echo_hdr *)(cBuffer + (IPH_HL(iphdrFrom) * 4));
if ((icmphdrFrom->id == 0xAFAF) && (icmphdrFrom->seqno == htons(usSeqRecv))) {
*piTTL = 0;
*piTTL = (u8_t)IPH_TTL(iphdrFrom);
return (ERROR_NONE);
}
}
iCnt--; /* 接收到错误的数据包太多 */
if (iCnt < 0) {
break; /* 退出 */
}
}
return (PX_ERROR);
}
/**
* Outbound TTL/HOPL check.
*/
static int
pxudp_ttl_expired(struct pbuf *p)
{
int ttl;
if (ip_current_is_v6()) {
ttl = IP6H_HOPLIM(ip6_current_header());
}
else {
ttl = IPH_TTL(ip_current_header());
}
if (RT_UNLIKELY(ttl <= 1)) {
int status = pbuf_header(p, ip_current_header_tot_len() + UDP_HLEN);
if (RT_LIKELY(status == 0)) {
if (ip_current_is_v6()) {
icmp6_time_exceeded(p, ICMP6_TE_HL);
}
else {
icmp_time_exceeded(p, ICMP_TE_TTL);
}
}
pbuf_free(p);
return 1;
}
return 0;
}
/* Ping using the raw ip */
static u8_t ping_recv(void *arg, struct raw_pcb *pcb, struct pbuf *p,
struct ip_addr *addr)
{
struct icmp_echo_hdr *iecho;
struct ip_hdr *ip = p->payload;
struct ping_info_t* ping_info = (struct ping_info_t*) arg;
uint32_t us;
if (pbuf_header( p, -PBUF_IP_HLEN)==0) {
iecho = p->payload;
if ((iecho->id == PING_ID) &&
(iecho->seqno == htons(ping_info->seq_num))) {
ping_info->last_rx_tm = timer_get_ms();
ping_info->num_rx++;
us = 1000 *
(ping_info->last_rx_tm - ping_info->last_tx_tm);
if (!ping_info->quiet)
printk("%d bytes from %s: icmp_seq=%d ttl=%d " \
"time=%d.%03d ms\n",
p->tot_len, ip2str(ip->src),
iecho->seqno,
IPH_TTL(ip),
us / 1000, us % 1000);
/* do some ping result processing */
ping_info->flags |= PING_REPLY;
}
}
pbuf_free(p);
return 1; /* eat the event */
}
uint8_t STC_FLASHMEM ICMP::raw_recv_fn(void *arg, raw_pcb *pcb, pbuf *p, ip_addr_t *addr) {
ICMP *this_ = (ICMP *)arg;
ip_hdr *iphdr = (ip_hdr *)p->payload;
uint16_t ttl = IPH_TTL(iphdr);
if (p->tot_len >= PBUF_IP_HLEN + sizeof(icmp_echo_hdr) && pbuf_header(p, -PBUF_IP_HLEN) == 0) {
icmp_echo_hdr *hdr = (icmp_echo_hdr *)p->payload;
if (ntohs(hdr->id) == this_->m_waitingID && ntohs(hdr->seqno) == this_->m_waitingSeq) {
this_->m_receivedBytes = p->tot_len;
this_->m_receivedTTL = ttl;
this_->m_queue.signal();
pbuf_free(p);
return 1;
}
}
return 0;
}
void
ip_debug_print(struct pbuf *p)
{
struct ip_hdr *iphdr = p->payload;
u8_t *payload;
payload = (u8_t *)iphdr + IP_HLEN/sizeof(u8_t);
DEBUGF(IP_DEBUG, ("IP header:\n"));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("|%2d |%2d | %2d | %4d | (v, hl, tos, len)\n",
IPH_V(iphdr),
IPH_HL(iphdr),
IPH_TOS(iphdr),
ntohs(IPH_LEN(iphdr))));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("| %5d |%d%d%d| %4d | (id, flags, offset)\n",
ntohs(IPH_ID(iphdr)),
ntohs(IPH_OFFSET(iphdr)) >> 15 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 14 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 13 & 1,
ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("| %2d | %2d | 0x%04x | (ttl, proto, chksum)\n",
IPH_TTL(iphdr),
IPH_PROTO(iphdr),
ntohs(IPH_CHKSUM(iphdr))));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("| %3ld | %3ld | %3ld | %3ld | (src)\n",
ntohl(iphdr->src.addr) >> 24 & 0xff,
ntohl(iphdr->src.addr) >> 16 & 0xff,
ntohl(iphdr->src.addr) >> 8 & 0xff,
ntohl(iphdr->src.addr) & 0xff));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("| %3ld | %3ld | %3ld | %3ld | (dest)\n",
ntohl(iphdr->dest.addr) >> 24 & 0xff,
ntohl(iphdr->dest.addr) >> 16 & 0xff,
ntohl(iphdr->dest.addr) >> 8 & 0xff,
ntohl(iphdr->dest.addr) & 0xff));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
}
void
ip_debug_print(struct pbuf *p)
{
struct ip_hdr *iphdr = p->payload;
u8_t *payload;
payload = (u8_t *)iphdr + IP_HLEN;
LWIP_DEBUGF(IP_DEBUG, ("IP header:\n"));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("|%2"S16_F" |%2"S16_F" | 0x%02"X16_F" | %5"U16_F" | (v, hl, tos, len)\n",
IPH_V(iphdr),
IPH_HL(iphdr),
IPH_TOS(iphdr),
ntohs(IPH_LEN(iphdr))));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %5"U16_F" |%"U16_F"%"U16_F"%"U16_F"| %4"U16_F" | (id, flags, offset)\n",
ntohs(IPH_ID(iphdr)),
ntohs(IPH_OFFSET(iphdr)) >> 15 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 14 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 13 & 1,
ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | 0x%04"X16_F" | (ttl, proto, chksum)\n",
IPH_TTL(iphdr),
IPH_PROTO(iphdr),
ntohs(IPH_CHKSUM(iphdr))));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (src)\n",
ip4_addr1(&iphdr->src),
ip4_addr2(&iphdr->src),
ip4_addr3(&iphdr->src),
ip4_addr4(&iphdr->src)));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (dest)\n",
ip4_addr1(&iphdr->dest),
ip4_addr2(&iphdr->dest),
ip4_addr3(&iphdr->dest),
ip4_addr4(&iphdr->dest)));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
}
static u8_t ping_recv(void *arg, struct raw_pcb *pcb,
struct pbuf *p, ip_addr_t *addr)
{
struct ip_hdr *iphdr;
struct icmp_echo_hdr *iecho;
LWIP_UNUSED_ARG(arg);
LWIP_UNUSED_ARG(pcb);
LWIP_UNUSED_ARG(addr);
LWIP_ASSERT("p != NULL", p != NULL);
if ((p->tot_len >= (PBUF_IP_HLEN + sizeof(struct icmp_echo_hdr)))) {
iphdr = (struct ip_hdr *)p->payload;
iecho = (struct icmp_echo_hdr *)(p->payload + (IPH_HL(iphdr) * 4));
if ((lns.ping_reply != NULL) &&
(iecho->id == PING_ID) &&
(iecho->seqno == htons(lns.ping_seq_num))) {
lns.ping_recv_tstamp = vmm_timer_timestamp();
lns.ping_reply->ripaddr[0] = ip4_addr1(&lns.ping_addr);
lns.ping_reply->ripaddr[1] = ip4_addr2(&lns.ping_addr);
lns.ping_reply->ripaddr[2] = ip4_addr3(&lns.ping_addr);
lns.ping_reply->ripaddr[3] = ip4_addr4(&lns.ping_addr);
lns.ping_reply->ttl = IPH_TTL(iphdr);
lns.ping_reply->len = p->tot_len - (IPH_HL(iphdr) * 4);
lns.ping_reply->seqno = lns.ping_seq_num;
vmm_completion_complete(&lns.ping_done);
/* Free the pbuf */
pbuf_free(p);
/* Eat the packet. lwIP should not process it. */
return 1;
}
}
/* Don't eat the packet. Let lwIP process it. */
return 0;
}
static void show_ip_pkt(struct pbuf *p)
{
struct ip_hdr *iphdr = (struct ip_hdr *)p->payload;
u8_t *payload;
payload = (u8_t *)iphdr + IP_HLEN;
_hx_printf("[%s]IP header:\r\n",__func__);
_hx_printf("+-------------------------------+\r\n");
_hx_printf("|%2"S16_F" |%2"S16_F" | 0x%02"X16_F" | %5"U16_F" | (v, hl, tos, len)\r\n",
IPH_V(iphdr),
IPH_HL(iphdr),
IPH_TOS(iphdr),
ntohs(IPH_LEN(iphdr)));
_hx_printf("+-------------------------------+\r\n");
_hx_printf("| %5"U16_F" |%"U16_F"%"U16_F"%"U16_F"| %4"U16_F" | (id, flags, offset)\r\n",
ntohs(IPH_ID(iphdr)),
ntohs(IPH_OFFSET(iphdr)) >> 15 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 14 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 13 & 1,
ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK);
_hx_printf("+-------------------------------+\r\n");
_hx_printf("| %3"U16_F" | %3"U16_F" | 0x%04"X16_F" | (ttl, proto, chksum)\r\n",
IPH_TTL(iphdr),
IPH_PROTO(iphdr),
ntohs(IPH_CHKSUM(iphdr)));
_hx_printf("+-------------------------------+\r\n");
_hx_printf("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (src)\r\n",
ip4_addr1_16(&iphdr->src),
ip4_addr2_16(&iphdr->src),
ip4_addr3_16(&iphdr->src),
ip4_addr4_16(&iphdr->src));
_hx_printf("+-------------------------------+\r\n");
_hx_printf("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (dest)\r\n",
ip4_addr1_16(&iphdr->dest),
ip4_addr2_16(&iphdr->dest),
ip4_addr3_16(&iphdr->dest),
ip4_addr4_16(&iphdr->dest));
_hx_printf("+-------------------------------+\r\n");
}
static void
pxudp_pcb_forward_outbound(struct pxudp *pxudp, struct pbuf *p,
ip_addr_t *addr, u16_t port)
{
int status;
LWIP_UNUSED_ARG(addr);
LWIP_UNUSED_ARG(port);
if (!pxudp->is_mapped && pxudp_ttl_expired(p)) {
return;
}
if (!ip_current_is_v6()) { /* IPv4 */
const struct ip_hdr *iph = ip_current_header();
int ttl, tos, df;
/*
* Different OSes have different socket options for DF.
* Unlike pxping.c, we can't use IP_HDRINCL here as it's only
* valid for SOCK_RAW.
*/
# define USE_DF_OPTION(_Optname) \
const int dfopt = _Optname; \
const char * const dfoptname = #_Optname;
#if defined(IP_MTU_DISCOVER) /* Linux */
USE_DF_OPTION(IP_MTU_DISCOVER);
#elif defined(IP_DONTFRAG) /* Solaris 11+, FreeBSD */
USE_DF_OPTION(IP_DONTFRAG);
#elif defined(IP_DONTFRAGMENT) /* Windows */
USE_DF_OPTION(IP_DONTFRAGMENT);
#else
USE_DF_OPTION(0);
#endif
ttl = IPH_TTL(iph);
if (!pxudp->is_mapped) {
LWIP_ASSERT1(ttl > 1);
--ttl;
}
if (ttl != pxudp->ttl) {
status = setsockopt(pxudp->sock, IPPROTO_IP, IP_TTL,
(char *)&ttl, sizeof(ttl));
if (RT_LIKELY(status == 0)) {
pxudp->ttl = ttl;
}
else {
DPRINTF(("IP_TTL: %R[sockerr]\n", SOCKERRNO()));
}
}
tos = IPH_TOS(iph);
if (tos != pxudp->tos) {
status = setsockopt(pxudp->sock, IPPROTO_IP, IP_TOS,
(char *)&tos, sizeof(tos));
if (RT_LIKELY(status == 0)) {
pxudp->tos = tos;
}
else {
DPRINTF(("IP_TOS: %R[sockerr]\n", SOCKERRNO()));
}
}
if (dfopt) {
df = (IPH_OFFSET(iph) & PP_HTONS(IP_DF)) != 0;
#if defined(IP_MTU_DISCOVER)
df = df ? IP_PMTUDISC_DO : IP_PMTUDISC_DONT;
#endif
if (df != pxudp->df) {
status = setsockopt(pxudp->sock, IPPROTO_IP, dfopt,
(char *)&df, sizeof(df));
if (RT_LIKELY(status == 0)) {
pxudp->df = df;
}
else {
DPRINTF(("%s: %R[sockerr]\n", dfoptname, SOCKERRNO()));
}
}
}
}
else { /* IPv6 */
const struct ip6_hdr *iph = ip6_current_header();
int ttl;
ttl = IP6H_HOPLIM(iph);
if (!pxudp->is_mapped) {
LWIP_ASSERT1(ttl > 1);
--ttl;
}
if (ttl != pxudp->ttl) {
status = setsockopt(pxudp->sock, IPPROTO_IPV6, IPV6_UNICAST_HOPS,
(char *)&ttl, sizeof(ttl));
if (RT_LIKELY(status == 0)) {
pxudp->ttl = ttl;
}
else {
DPRINTF(("IPV6_UNICAST_HOPS: %R[sockerr]\n", SOCKERRNO()));
}
}
}
if (pxudp->pcb->local_port == 53) {
++pxudp->count;
}
proxy_sendto(pxudp->sock, p, NULL, 0);
pbuf_free(p);
}
/*-----------------------------------------------------------------------------------*/
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);
ip_output_if(p, &(iphdr->src), IP_HDRINCL,
IPH_TTL(iphdr), IP_PROTO_ICMP, inp);
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);
}
/**
* This function is called by the network interface device driver when
* an IP packet is received. The function does the basic checks of the
* IP header such as packet size being at least larger than the header
* size etc. If the packet was not destined for us, the packet is
* forwarded (using ip_forward). The IP checksum is always checked.
*
* Finally, the packet is sent to the upper layer protocol input function.
*
* @param p the received IP packet (p->payload points to IP header)
* @param inp the netif on which this packet was received
* @return ERR_OK if the packet was processed (could return ERR_* if it wasn't
* processed, but currently always returns ERR_OK)
*/
err_t
ip_input(struct pbuf *p, struct netif *inp)
{
struct ip_hdr *iphdr;
struct netif *netif;
u16_t iphdr_hlen;
u16_t iphdr_len;
#if LWIP_DHCP
int check_ip_src=1;
#endif /* LWIP_DHCP */
IP_STATS_INC(ip.recv);
snmp_inc_ipinreceives();
/* identify the IP header */
iphdr = p->payload;
if (IPH_V(iphdr) != 4) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_WARNING, ("IP packet dropped due to bad version number %"U16_F"\n", IPH_V(iphdr)));
ip_debug_print(p);
pbuf_free(p);
IP_STATS_INC(ip.err);
IP_STATS_INC(ip.drop);
snmp_inc_ipinhdrerrors();
return ERR_OK;
}
/* obtain IP header length in number of 32-bit words */
iphdr_hlen = IPH_HL(iphdr);
/* calculate IP header length in bytes */
iphdr_hlen *= 4;
/* obtain ip length in bytes */
iphdr_len = ntohs(IPH_LEN(iphdr));
/* header length exceeds first pbuf length, or ip length exceeds total pbuf length? */
if ((iphdr_hlen > p->len) || (iphdr_len > p->tot_len)) {
if (iphdr_hlen > p->len) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS,
("IP header (len %"U16_F") does not fit in first pbuf (len %"U16_F"), IP packet dropped.\n",
iphdr_hlen, p->len));
}
if (iphdr_len > p->tot_len) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS,
("IP (len %"U16_F") is longer than pbuf (len %"U16_F"), IP packet dropped.\n",
iphdr_len, p->tot_len));
}
/* free (drop) packet pbufs */
pbuf_free(p);
IP_STATS_INC(ip.lenerr);
IP_STATS_INC(ip.drop);
snmp_inc_ipindiscards();
return ERR_OK;
}
/* verify checksum */
#if CHECKSUM_CHECK_IP
if (inet_chksum(iphdr, iphdr_hlen) != 0) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS,
("Checksum (0x%"X16_F") failed, IP packet dropped.\n", inet_chksum(iphdr, iphdr_hlen)));
ip_debug_print(p);
pbuf_free(p);
IP_STATS_INC(ip.chkerr);
IP_STATS_INC(ip.drop);
snmp_inc_ipinhdrerrors();
return ERR_OK;
}
#endif
/* Trim pbuf. This should have been done at the netif layer,
* but we'll do it anyway just to be sure that its done. */
pbuf_realloc(p, iphdr_len);
/* match packet against an interface, i.e. is this packet for us? */
#if LWIP_IGMP
if (ip_addr_ismulticast(&(iphdr->dest))) {
if ((inp->flags & NETIF_FLAG_IGMP) && (igmp_lookfor_group(inp, &(iphdr->dest)))) {
netif = inp;
} else {
netif = NULL;
}
} else
#endif /* LWIP_IGMP */
{
/* start trying with inp. if that's not acceptable, start walking the
list of configured netifs.
'first' is used as a boolean to mark whether we started walking the list */
int first = 1;
netif = inp;
do {
LWIP_DEBUGF(IP_DEBUG, ("ip_input: iphdr->dest 0x%"X32_F" netif->ip_addr 0x%"X32_F" (0x%"X32_F", 0x%"X32_F", 0x%"X32_F")\n",
iphdr->dest.addr, netif->ip_addr.addr,
iphdr->dest.addr & netif->netmask.addr,
netif->ip_addr.addr & netif->netmask.addr,
iphdr->dest.addr & ~(netif->netmask.addr)));
/* interface is up and configured? */
if ((netif_is_up(netif)) && (!ip_addr_isany(&(netif->ip_addr)))) {
/* unicast to this interface address? */
if (ip_addr_cmp(&(iphdr->dest), &(netif->ip_addr)) ||
/* or broadcast on this interface network address? */
ip_addr_isbroadcast(&(iphdr->dest), netif)) {
LWIP_DEBUGF(IP_DEBUG, ("ip_input: packet accepted on interface %c%c\n",
netif->name[0], netif->name[1]));
/* break out of for loop */
break;
}
}
if (first) {
first = 0;
netif = netif_list;
} else {
netif = netif->next;
}
if (netif == inp) {
netif = netif->next;
}
} while(netif != NULL);
}
#if LWIP_DHCP
/* Pass DHCP messages regardless of destination address. DHCP traffic is addressed
* using link layer addressing (such as Ethernet MAC) so we must not filter on IP.
* According to RFC 1542 section 3.1.1, referred by RFC 2131).
*/
if (netif == NULL) {
/* remote port is DHCP server? */
if (IPH_PROTO(iphdr) == IP_PROTO_UDP) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_TRACE, ("ip_input: UDP packet to DHCP client port %"U16_F"\n",
ntohs(((struct udp_hdr *)((u8_t *)iphdr + iphdr_hlen))->dest)));
if (ntohs(((struct udp_hdr *)((u8_t *)iphdr + iphdr_hlen))->dest) == DHCP_CLIENT_PORT) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_TRACE, ("ip_input: DHCP packet accepted.\n"));
netif = inp;
check_ip_src = 0;
}
}
}
#endif /* LWIP_DHCP */
/* broadcast or multicast packet source address? Compliant with RFC 1122: 3.2.1.3 */
#if LWIP_DHCP
/* DHCP servers need 0.0.0.0 to be allowed as source address (RFC 1.1.2.2: 3.2.1.3/a) */
if (check_ip_src && (iphdr->src.addr != 0))
#endif /* LWIP_DHCP */
{ if ((ip_addr_isbroadcast(&(iphdr->src), inp)) ||
(ip_addr_ismulticast(&(iphdr->src)))) {
/* packet source is not valid */
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_WARNING, ("ip_input: packet source is not valid.\n"));
/* free (drop) packet pbufs */
pbuf_free(p);
IP_STATS_INC(ip.drop);
snmp_inc_ipinaddrerrors();
snmp_inc_ipindiscards();
return ERR_OK;
}
}
/* packet not for us? */
if (netif == NULL) {
/* packet not for us, route or discard */
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_TRACE, ("ip_input: packet not for us.\n"));
#if IP_FORWARD
/* non-broadcast packet? */
if (!ip_addr_isbroadcast(&(iphdr->dest), inp)) {
/* try to forward IP packet on (other) interfaces */
ip_forward(p, iphdr, inp);
} else
#endif /* IP_FORWARD */
{
snmp_inc_ipinaddrerrors();
snmp_inc_ipindiscards();
}
pbuf_free(p);
return ERR_OK;
}
/* packet consists of multiple fragments? */
if ((IPH_OFFSET(iphdr) & htons(IP_OFFMASK | IP_MF)) != 0) {
#if IP_REASSEMBLY /* packet fragment reassembly code present? */
LWIP_DEBUGF(IP_DEBUG, ("IP packet is a fragment (id=0x%04"X16_F" tot_len=%"U16_F" len=%"U16_F" MF=%"U16_F" offset=%"U16_F"), calling ip_reass()\n",
ntohs(IPH_ID(iphdr)), p->tot_len, ntohs(IPH_LEN(iphdr)), !!(IPH_OFFSET(iphdr) & htons(IP_MF)), (ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK)*8));
/* reassemble the packet*/
p = ip_reass(p);
/* packet not fully reassembled yet? */
if (p == NULL) {
return ERR_OK;
}
iphdr = p->payload;
#else /* IP_REASSEMBLY == 0, no packet fragment reassembly code present */
pbuf_free(p);
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("IP packet dropped since it was fragmented (0x%"X16_F") (while IP_REASSEMBLY == 0).\n",
ntohs(IPH_OFFSET(iphdr))));
IP_STATS_INC(ip.opterr);
IP_STATS_INC(ip.drop);
/* unsupported protocol feature */
snmp_inc_ipinunknownprotos();
return ERR_OK;
#endif /* IP_REASSEMBLY */
}
#if IP_OPTIONS_ALLOWED == 0 /* no support for IP options in the IP header? */
#if LWIP_IGMP
/* there is an extra "router alert" option in IGMP messages which we allow for but do not police */
if((iphdr_hlen > IP_HLEN && (IPH_PROTO(iphdr) != IP_PROTO_IGMP)) {
#else
if (iphdr_hlen > IP_HLEN) {
#endif /* LWIP_IGMP */
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("IP packet dropped since there were IP options (while IP_OPTIONS_ALLOWED == 0).\n"));
pbuf_free(p);
IP_STATS_INC(ip.opterr);
IP_STATS_INC(ip.drop);
/* unsupported protocol feature */
snmp_inc_ipinunknownprotos();
return ERR_OK;
}
#endif /* IP_OPTIONS_ALLOWED == 0 */
/* send to upper layers */
LWIP_DEBUGF(IP_DEBUG, ("ip_input: \n"));
ip_debug_print(p);
LWIP_DEBUGF(IP_DEBUG, ("ip_input: p->len %"U16_F" p->tot_len %"U16_F"\n", p->len, p->tot_len));
current_netif = inp;
current_header = iphdr;
#if LWIP_RAW
/* raw input did not eat the packet? */
if (raw_input(p, inp) == 0)
#endif /* LWIP_RAW */
{
switch (IPH_PROTO(iphdr)) {
#if LWIP_UDP
case IP_PROTO_UDP:
#if LWIP_UDPLITE
case IP_PROTO_UDPLITE:
#endif /* LWIP_UDPLITE */
snmp_inc_ipindelivers();
udp_input(p, inp);
break;
#endif /* LWIP_UDP */
#if LWIP_TCP
case IP_PROTO_TCP:
snmp_inc_ipindelivers();
tcp_input(p, inp);
break;
#endif /* LWIP_TCP */
#if LWIP_ICMP
case IP_PROTO_ICMP:
snmp_inc_ipindelivers();
icmp_input(p, inp);
break;
#endif /* LWIP_ICMP */
#if LWIP_IGMP
case IP_PROTO_IGMP:
igmp_input(p,inp,&(iphdr->dest));
break;
#endif /* LWIP_IGMP */
default:
#if LWIP_ICMP
/* send ICMP destination protocol unreachable unless is was a broadcast */
if (!ip_addr_isbroadcast(&(iphdr->dest), inp) &&
!ip_addr_ismulticast(&(iphdr->dest))) {
p->payload = iphdr;
icmp_dest_unreach(p, ICMP_DUR_PROTO);
}
#endif /* LWIP_ICMP */
pbuf_free(p);
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("Unsupported transport protocol %"U16_F"\n", IPH_PROTO(iphdr)));
IP_STATS_INC(ip.proterr);
IP_STATS_INC(ip.drop);
snmp_inc_ipinunknownprotos();
}
}
current_netif = NULL;
current_header = NULL;
return ERR_OK;
}
/**
* Sends an IP packet on a network interface. This function constructs
* the IP header and calculates the IP header checksum. If the source
* IP address is NULL, the IP address of the outgoing network
* interface is filled in as source address.
* If the destination IP address is IP_HDRINCL, p is assumed to already
* include an IP header and p->payload points to it instead of the data.
*
* @param p the packet to send (p->payload points to the data, e.g. next
protocol header; if dest == IP_HDRINCL, p already includes an IP
header and p->payload points to that IP header)
* @param src the source IP address to send from (if src == IP_ADDR_ANY, the
* IP address of the netif used to send is used as source address)
* @param dest the destination IP address to send the packet to
* @param ttl the TTL value to be set in the IP header
* @param tos the TOS value to be set in the IP header
* @param proto the PROTOCOL to be set in the IP header
* @param netif the netif on which to send this packet
* @return ERR_OK if the packet was sent OK
* ERR_BUF if p doesn't have enough space for IP/LINK headers
* returns errors returned by netif->output
*
* @note ip_id: RFC791 "some host may be able to simply use
* unique identifiers independent of destination"
*/
err_t
ip_output_if(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
u8_t ttl, u8_t tos,
u8_t proto, struct netif *netif)
{
#if IP_OPTIONS_SEND
return ip_output_if_opt(p, src, dest, ttl, tos, proto, netif, NULL, 0);
}
/**
* Same as ip_output_if() but with the possibility to include IP options:
*
* @ param ip_options pointer to the IP options, copied into the IP header
* @ param optlen length of ip_options
*/
err_t ip_output_if_opt(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
u8_t ttl, u8_t tos, u8_t proto, struct netif *netif, void *ip_options,
u16_t optlen)
{
#endif /* IP_OPTIONS_SEND */
struct ip_hdr *iphdr;
static u16_t ip_id = 0;
snmp_inc_ipoutrequests();
/* Should the IP header be generated or is it already included in p? */
if (dest != IP_HDRINCL) {
u16_t ip_hlen = IP_HLEN;
#if IP_OPTIONS_SEND
u16_t optlen_aligned = 0;
if (optlen != 0) {
/* round up to a multiple of 4 */
optlen_aligned = ((optlen + 3) & ~3);
ip_hlen += optlen_aligned;
/* First write in the IP options */
if (pbuf_header(p, optlen_aligned)) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("ip_output_if_opt: not enough room for IP options in pbuf\n"));
IP_STATS_INC(ip.err);
snmp_inc_ipoutdiscards();
return ERR_BUF;
}
MEMCPY(p->payload, ip_options, optlen);
if (optlen < optlen_aligned) {
/* zero the remaining bytes */
memset(((char*)p->payload) + optlen, 0, optlen_aligned - optlen);
}
}
#endif /* IP_OPTIONS_SEND */
/* generate IP header */
if (pbuf_header(p, IP_HLEN)) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("ip_output: not enough room for IP header in pbuf\n"));
IP_STATS_INC(ip.err);
snmp_inc_ipoutdiscards();
return ERR_BUF;
}
iphdr = p->payload;
LWIP_ASSERT("check that first pbuf can hold struct ip_hdr",
(p->len >= sizeof(struct ip_hdr)));
IPH_TTL_SET(iphdr, ttl);
IPH_PROTO_SET(iphdr, proto);
ip_addr_set(&(iphdr->dest), dest);
IPH_VHLTOS_SET(iphdr, 4, ip_hlen / 4, tos);
IPH_LEN_SET(iphdr, htons(p->tot_len));
IPH_OFFSET_SET(iphdr, 0);
IPH_ID_SET(iphdr, htons(ip_id));
++ip_id;
if (ip_addr_isany(src)) {
ip_addr_set(&(iphdr->src), &(netif->ip_addr));
} else {
ip_addr_set(&(iphdr->src), src);
}
IPH_CHKSUM_SET(iphdr, 0);
#if CHECKSUM_GEN_IP
IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, ip_hlen));
#endif
} else {
/* IP header already included in p */
iphdr = p->payload;
dest = &(iphdr->dest);
}
IP_STATS_INC(ip.xmit);
LWIP_DEBUGF(IP_DEBUG, ("ip_output_if: %c%c%"U16_F"\n", netif->name[0], netif->name[1], netif->num));
ip_debug_print(p);
#if ENABLE_LOOPBACK
if (ip_addr_cmp(dest, &netif->ip_addr)) {
/* Packet to self, enqueue it for loopback */
LWIP_DEBUGF(IP_DEBUG, ("netif_loop_output()"));
return netif_loop_output(netif, p, dest);
}
#endif /* ENABLE_LOOPBACK */
#if IP_FRAG
/* don't fragment if interface has mtu set to 0 [loopif] */
if (netif->mtu && (p->tot_len > netif->mtu)) {
return ip_frag(p,netif,dest);
}
#endif
LWIP_DEBUGF(IP_DEBUG, ("netif->output()"));
return netif->output(netif, p, dest);
}
/**
* Simple interface to ip_output_if. It finds the outgoing network
* interface and calls upon ip_output_if to do the actual work.
*
* @param p the packet to send (p->payload points to the data, e.g. next
protocol header; if dest == IP_HDRINCL, p already includes an IP
header and p->payload points to that IP header)
* @param src the source IP address to send from (if src == IP_ADDR_ANY, the
* IP address of the netif used to send is used as source address)
* @param dest the destination IP address to send the packet to
* @param ttl the TTL value to be set in the IP header
* @param tos the TOS value to be set in the IP header
* @param proto the PROTOCOL to be set in the IP header
*
* @return ERR_RTE if no route is found
* see ip_output_if() for more return values
*/
err_t
ip_output(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
u8_t ttl, u8_t tos, u8_t proto)
{
struct netif *netif;
if ((netif = ip_route(dest)) == NULL) {
LWIP_DEBUGF(IP_DEBUG, ("ip_output: No route to 0x%"X32_F"\n", dest->addr));
IP_STATS_INC(ip.rterr);
return ERR_RTE;
}
return ip_output_if(p, src, dest, ttl, tos, proto, netif);
}
#if LWIP_NETIF_HWADDRHINT
/** Like ip_output, but takes and addr_hint pointer that is passed on to netif->addr_hint
* before calling ip_output_if.
*
* @param p the packet to send (p->payload points to the data, e.g. next
protocol header; if dest == IP_HDRINCL, p already includes an IP
header and p->payload points to that IP header)
* @param src the source IP address to send from (if src == IP_ADDR_ANY, the
* IP address of the netif used to send is used as source address)
* @param dest the destination IP address to send the packet to
* @param ttl the TTL value to be set in the IP header
* @param tos the TOS value to be set in the IP header
* @param proto the PROTOCOL to be set in the IP header
* @param addr_hint address hint pointer set to netif->addr_hint before
* calling ip_output_if()
*
* @return ERR_RTE if no route is found
* see ip_output_if() for more return values
*/
err_t
ip_output_hinted(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
u8_t ttl, u8_t tos, u8_t proto, u8_t *addr_hint)
{
struct netif *netif;
err_t err;
if ((netif = ip_route(dest)) == NULL) {
LWIP_DEBUGF(IP_DEBUG, ("ip_output: No route to 0x%"X32_F"\n", dest->addr));
IP_STATS_INC(ip.rterr);
return ERR_RTE;
}
netif->addr_hint = addr_hint;
err = ip_output_if(p, src, dest, ttl, tos, proto, netif);
netif->addr_hint = NULL;
return err;
}
#endif /* LWIP_NETIF_HWADDRHINT*/
#if IP_DEBUG
/* Print an IP header by using LWIP_DEBUGF
* @param p an IP packet, p->payload pointing to the IP header
*/
void
ip_debug_print(struct pbuf *p)
{
struct ip_hdr *iphdr = p->payload;
u8_t *payload;
payload = (u8_t *)iphdr + IP_HLEN;
LWIP_DEBUGF(IP_DEBUG, ("IP header:\n"));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("|%2"S16_F" |%2"S16_F" | 0x%02"X16_F" | %5"U16_F" | (v, hl, tos, len)\n",
IPH_V(iphdr),
IPH_HL(iphdr),
IPH_TOS(iphdr),
ntohs(IPH_LEN(iphdr))));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %5"U16_F" |%"U16_F"%"U16_F"%"U16_F"| %4"U16_F" | (id, flags, offset)\n",
ntohs(IPH_ID(iphdr)),
ntohs(IPH_OFFSET(iphdr)) >> 15 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 14 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 13 & 1,
ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | 0x%04"X16_F" | (ttl, proto, chksum)\n",
IPH_TTL(iphdr),
IPH_PROTO(iphdr),
ntohs(IPH_CHKSUM(iphdr))));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (src)\n",
ip4_addr1(&iphdr->src),
ip4_addr2(&iphdr->src),
ip4_addr3(&iphdr->src),
ip4_addr4(&iphdr->src)));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (dest)\n",
ip4_addr1(&iphdr->dest),
ip4_addr2(&iphdr->dest),
ip4_addr3(&iphdr->dest),
ip4_addr4(&iphdr->dest)));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
}
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);
}
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;
}
/**
* ICMP Echo Request in pbuf "p" is to be proxied.
*/
static void
pxping_recv4(void *arg, struct pbuf *p)
{
struct pxping *pxping = (struct pxping *)arg;
const struct ip_hdr *iph;
const struct icmp_echo_hdr *icmph;
u16_t iphlen;
size_t bufsize;
struct pong4 *pong;
IPAddr dst;
int mapped;
int ttl;
IP_OPTION_INFORMATION opts;
void *reqdata;
size_t reqsize;
int status;
pong = NULL;
iphlen = ip_current_header_tot_len();
if (RT_UNLIKELY(iphlen != IP_HLEN)) { /* we don't do options */
goto out;
}
iph = (const struct ip_hdr *)ip_current_header();
icmph = (const struct icmp_echo_hdr *)p->payload;
mapped = pxremap_outbound_ip4((ip_addr_t *)&dst, (ip_addr_t *)&iph->dest);
if (RT_UNLIKELY(mapped == PXREMAP_FAILED)) {
goto out;
}
ttl = IPH_TTL(iph);
if (mapped == PXREMAP_ASIS) {
if (RT_UNLIKELY(ttl == 1)) {
status = pbuf_header(p, iphlen); /* back to IP header */
if (RT_LIKELY(status == 0)) {
icmp_time_exceeded(p, ICMP_TE_TTL);
}
goto out;
}
--ttl;
}
status = pbuf_header(p, -(u16_t)sizeof(*icmph)); /* to ping payload */
if (RT_UNLIKELY(status != 0)) {
goto out;
}
bufsize = sizeof(ICMP_ECHO_REPLY) + p->tot_len;
pong = (struct pong4 *)malloc(sizeof(*pong) - sizeof(pong->buf) + bufsize);
if (RT_UNLIKELY(pong == NULL)) {
goto out;
}
pong->bufsize = bufsize;
pong->netif = pxping->netif;
memcpy(&pong->reqiph, iph, sizeof(*iph));
memcpy(&pong->reqicmph, icmph, sizeof(*icmph));
reqsize = p->tot_len;
if (p->next == NULL) {
/* single pbuf can be directly used as request data source */
reqdata = p->payload;
}
else {
/* data from pbuf chain must be concatenated */
pbuf_copy_partial(p, pong->buf, p->tot_len, 0);
reqdata = pong->buf;
}
opts.Ttl = ttl;
opts.Tos = IPH_TOS(iph); /* affected by DisableUserTOSSetting key */
opts.Flags = (IPH_OFFSET(iph) & PP_HTONS(IP_DF)) != 0 ? IP_FLAG_DF : 0;
opts.OptionsSize = 0;
opts.OptionsData = 0;
status = IcmpSendEcho2(pxping->hdl4, NULL,
pxping->callback4, pong,
dst, reqdata, (WORD)reqsize, &opts,
pong->buf, (DWORD)pong->bufsize,
5 * 1000 /* ms */);
if (RT_UNLIKELY(status != 0)) {
DPRINTF(("IcmpSendEcho2: unexpected status %d\n", status));
goto out;
}
else if ((status = GetLastError()) != ERROR_IO_PENDING) {
int code;
DPRINTF(("IcmpSendEcho2: error %d\n", status));
switch (status) {
case ERROR_NETWORK_UNREACHABLE:
code = ICMP_DUR_NET;
break;
case ERROR_HOST_UNREACHABLE:
code = ICMP_DUR_HOST;
break;
default:
code = -1;
break;
}
if (code != -1) {
/* move payload back to IP header */
status = pbuf_header(p, (u16_t)(sizeof(*icmph) + iphlen));
if (RT_LIKELY(status == 0)) {
icmp_dest_unreach(p, code);
}
}
goto out;
}
pong = NULL; /* callback owns it now */
out:
if (pong != NULL) {
free(pong);
}
pbuf_free(p);
}