/************************************************************************
* NAME: fnet_icmp_notify_protocol
*
* DESCRIPTION: Upper protocol notification..
*************************************************************************/
static void fnet_icmp_notify_protocol(fnet_prot_notify_t prot_cmd, fnet_netbuf_t *nb)
{
fnet_icmp_err_header_t *hdr_err = (fnet_icmp_err_header_t *)nb->data_ptr;
fnet_ip_header_t *ip_header = &hdr_err->ip;
fnet_size_t hdr_err_length = sizeof(fnet_icmp_err_header_t) /*+ ((FNET_IP_HEADER_GET_HEADER_LENGTH(ip_header) << 2) - sizeof(fnet_ip_header_t))*/;
fnet_size_t hdr_err_data_length = hdr_err_length+8u; /* 8 bytes is enough for transport protocol (port numbers).*/
fnet_prot_if_t *protocol;
if(nb->total_length < hdr_err_data_length)
{
goto DISCARD;
}
if(nb->total_length > hdr_err_data_length)
{
fnet_netbuf_trim(&nb, (fnet_int32_t)(hdr_err_data_length - nb->total_length));
}
if(fnet_netbuf_pullup(&nb, nb->total_length) == FNET_ERR) /* The header must reside in contiguous area of memory.*/
{
goto DISCARD;
}
hdr_err = (fnet_icmp_err_header_t *)nb->data_ptr;
ip_header = &hdr_err->ip;
if((protocol = fnet_prot_find(AF_INET, SOCK_UNSPEC, (fnet_uint32_t)hdr_err->ip.protocol)) != 0)
{
if(protocol->prot_control_input)
{
struct sockaddr err_src_addr;
struct sockaddr err_dest_addr;
/* Prepare addreses for upper protocol.*/
fnet_ip_set_socket_addr(ip_header, &err_src_addr, &err_dest_addr );
fnet_netbuf_trim(&nb, (fnet_int32_t)(hdr_err_length)); /* Cut the ICMP error header.*/
protocol->prot_control_input(prot_cmd, &err_src_addr, &err_dest_addr, nb);
}
}
DISCARD:
fnet_netbuf_free_chain(nb);
}
/************************************************************************
* NAME: fnet_icmp_input
*
* DESCRIPTION: ICMP input function.
*************************************************************************/
static void fnet_icmp_input(fnet_netif_t *netif, struct sockaddr *src_addr, struct sockaddr *dest_addr, fnet_netbuf_t *nb, fnet_netbuf_t *ip4_nb)
{
fnet_icmp_header_t *hdr;
fnet_prot_notify_t prot_cmd;
fnet_ip4_addr_t src_ip;
fnet_ip4_addr_t dest_ip;
fnet_netbuf_free_chain(ip4_nb); /* Not used.*/
if((netif != 0) && (nb != 0) )
{
if(fnet_netbuf_pullup(&nb, sizeof(fnet_icmp_header_t)) == FNET_ERR) /* The header must reside in contiguous area of memory. */
{
goto DISCARD;
}
hdr = (fnet_icmp_header_t *)nb->data_ptr;
src_ip = ((struct sockaddr_in*)(src_addr))->sin_addr.s_addr;
dest_ip = ((struct sockaddr_in*)(dest_addr))->sin_addr.s_addr;
if(
#if FNET_CFG_CPU_ETH_HW_RX_PROTOCOL_CHECKSUM || FNET_CFG_CPU_ETH_HW_TX_PROTOCOL_CHECKSUM
((nb->flags & FNET_NETBUF_FLAG_HW_PROTOCOL_CHECKSUM) == 0) &&
#endif
(fnet_checksum(nb, nb->total_length))
|| (fnet_ip_addr_is_broadcast(src_ip, netif))
|| FNET_IP4_ADDR_IS_MULTICAST(src_ip))
{
goto DISCARD;
}
fnet_icmp_trace("RX", hdr);
switch(hdr->type)
{
/**************************
* ICMP Request Processing
**************************/
case FNET_ICMP_ECHO:
if((nb->total_length < sizeof(fnet_icmp_echo_header_t)) ||
/* An ICMP Echo Request destined to an IP broadcast or IP
* multicast address MAY be silently discarded.(RFC1122)*/
(fnet_ip_addr_is_broadcast(dest_ip, netif)) || FNET_IP4_ADDR_IS_MULTICAST(dest_ip))
{
goto DISCARD;
}
hdr->type = FNET_ICMP_ECHOREPLY;
fnet_icmp_output(netif, dest_ip, src_ip, nb);
break;
#if 0 /* Optional functionality.*/
/************************
* Time Stamp Query
************************/
case FNET_ICMP_TSTAMP:
/* The header must reside in contiguous area of memory. */
if(fnet_netbuf_pullup(&nb, sizeof(fnet_icmp_timestamp_header_t)) == FNET_ERR)
{
goto DISCARD;
}
hdr = nb->data_ptr;
hdr->type = FNET_ICMP_TSTAMPREPLY;
/* All times are in milliseconds since the stack timer start modulo 1 day.
* The high-order bit is set as the time value is recorded in nonstandard units. */
((fnet_icmp_timestamp_header_t *)hdr)->receive_timestamp
= fnet_htonl(((fnet_timer_ticks() * FNET_TIMER_PERIOD_MS) % (24 * 60 * 60 * 1000)) | (0x80000000));
dest_ip = netif->ip4_addr.address;
fnet_icmp_output(netif, dest_ip, src_ip, nb);
break;
/************************
* Address Mask Query
************************/
case FNET_ICMP_MASKREQ:
/* The header must reside in contiguous area of memory*/
if(fnet_netbuf_pullup(&nb, sizeof(fnet_icmp_mask_header_t)) == FNET_ERR)
{
goto DISCARD;
}
hdr = nb->data_ptr;
hdr->type = FNET_ICMP_MASKREPLY;
((fnet_icmp_mask_header_t *)hdr)->mask = netif->ip4_addr.subnetmask;
dest_ip = netif->ip4_addr.address;
fnet_icmp_output(netif, dest_ip, src_ip, nb);
break;
#endif
/**************************
* ICMP Error Processing
**************************/
case FNET_ICMP_UNREACHABLE:
switch(hdr->code)
{
case FNET_ICMP_UNREACHABLE_NET: /* net unreachable */
case FNET_ICMP_UNREACHABLE_NET_UNKNOWN: /* unknown net */
case FNET_ICMP_UNREACHABLE_NET_PROHIB: /* prohibited access */
case FNET_ICMP_UNREACHABLE_TOSNET: /* bad tos for net */
prot_cmd = FNET_PROT_NOTIFY_UNREACH_NET;
break;
case FNET_ICMP_UNREACHABLE_HOST: /* host unreachable */
case FNET_ICMP_UNREACHABLE_HOST_UNKNOWN: /* unknown host */
case FNET_ICMP_UNREACHABLE_ISOLATED: /* src host isolated */
case FNET_ICMP_UNREACHABLE_HOST_PROHIB: /* ditto */
case FNET_ICMP_UNREACHABLE_TOSHOST: /* bad tos for host */
prot_cmd = FNET_PROT_NOTIFY_UNREACH_HOST;
break;
case FNET_ICMP_UNREACHABLE_PROTOCOL: /* protocol unreachable */
prot_cmd = FNET_PROT_NOTIFY_UNREACH_PROTOCOL;
break;
case FNET_ICMP_UNREACHABLE_PORT: /* port unreachable */
prot_cmd = FNET_PROT_NOTIFY_UNREACH_PORT;
break;
case FNET_ICMP_UNREACHABLE_SRCFAIL: /* source route failed */
prot_cmd = FNET_PROT_NOTIFY_UNREACH_SRCFAIL;
break;
case FNET_ICMP_UNREACHABLE_NEEDFRAG: /* fragmentation needed and DF set*/
prot_cmd = FNET_PROT_NOTIFY_MSGSIZE;
break;
default:
goto DISCARD;
}
fnet_icmp_notify_protocol(prot_cmd, nb); /* Protocol notification.*/
break;
case FNET_ICMP_TIMXCEED:
switch(hdr->code)
{
case FNET_ICMP_TIMXCEED_INTRANS: /* time to live exceeded in transit (ttl==0)*/
prot_cmd = FNET_PROT_NOTIFY_TIMXCEED_INTRANS;
break;
case FNET_ICMP_TIMXCEED_REASS: /* fragment reassembly time exceeded (ttl==0)*/
prot_cmd = FNET_PROT_NOTIFY_TIMXCEED_REASS;
break;
default:
goto DISCARD;
}
fnet_icmp_notify_protocol(prot_cmd, nb); /* Protocol notification.*/
break;
case FNET_ICMP_PARAMPROB: /* Parameter Problem Message.*/
if(hdr->code > 1u)
{
goto DISCARD;
}
prot_cmd = FNET_PROT_NOTIFY_PARAMPROB;
fnet_icmp_notify_protocol(prot_cmd, nb); /* Protocol notification.*/
break;
case FNET_ICMP_SOURCEQUENCH: /* Source Quench Message; packet lost, slow down.*/
if(hdr->code)
{
goto DISCARD;
}
prot_cmd = FNET_PROT_NOTIFY_QUENCH;
fnet_icmp_notify_protocol(prot_cmd, nb); /* Protocol notification.*/
break;
/************************
* Ignore others
************************/
/*
case FNET_ICMP_REDIRECT:
case FNET_ICMP_ECHOREPLY:
case FNET_ICMP_ROUTERADVERT:
case FNET_ICMP_ROUTERSOLICIT:
case FNET_ICMP_TSTAMPREPLY:
case FNET_ICMP_IREQREPLY:
case FNET_ICMP_MASKREPLY:*/
default:
goto DISCARD;
}/* switch(hdr->type) */
}
else
{
DISCARD:
fnet_netbuf_free_chain(nb);
}
}
/************************************************************************
* NAME: fnet_icmp6_input
*
* DESCRIPTION: ICMPv6 input function.
*************************************************************************/
static void fnet_icmp6_input(fnet_netif_t *netif, struct sockaddr *src_addr, struct sockaddr *dest_addr, fnet_netbuf_t *nb, fnet_netbuf_t *ip6_nb)
{
fnet_icmp6_header_t *hdr;
fnet_netbuf_t *tmp_nb;
unsigned short sum;
fnet_ip6_addr_t *src_ip;
fnet_ip6_addr_t *dest_ip;
fnet_prot_notify_t prot_cmd;
if((netif != 0) && (nb != 0))
{
/* The header must reside in contiguous area of memory. */
if((tmp_nb = fnet_netbuf_pullup(nb, sizeof(fnet_icmp6_header_t))) == 0)
{
goto DISCARD;
}
nb = tmp_nb;
hdr = nb->data_ptr;
dest_ip = &((struct sockaddr_in6 *)(dest_addr))->sin6_addr.s6_addr;
src_ip = &((struct sockaddr_in6 *)(src_addr))->sin6_addr.s6_addr;
/* Drop Multicast loopback.*/
#if FNET_CFG_LOOPBACK
if ((netif == FNET_LOOP_IF) && FNET_IP6_ADDR_IS_MULTICAST(dest_ip))
{
goto DISCARD;
}
#endif /* FNET_CFG_LOOPBACK */
/* Verify the checksum. */
sum = fnet_checksum_pseudo_start( nb, FNET_HTONS((unsigned short)FNET_IP_PROTOCOL_ICMP6), (unsigned short)nb->total_length );
sum = fnet_checksum_pseudo_end( sum, (char *)src_ip, (char *)dest_ip, sizeof(fnet_ip6_addr_t) );
if(sum)
goto DISCARD;
/************************************************************
* Process incoming ICMPv6 packets.
*************************************************************/
switch (hdr->type)
{
/**************************
* Neighbor Solicitation.
**************************/
case FNET_ICMP6_TYPE_NEIGHBOR_SOLICITATION:
fnet_nd6_neighbor_solicitation_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
/**************************
* Neighbor Advertisemnt.
**************************/
case FNET_ICMP6_TYPE_NEIGHBOR_ADVERTISEMENT:
fnet_nd6_neighbor_advertisement_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
/**************************
* Router Advertisemnt.
**************************/
case FNET_ICMP6_TYPE_ROUTER_ADVERTISEMENT:
fnet_nd6_router_advertisement_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
/**************************
* Router Advertisemnt.
**************************/
case FNET_ICMP6_TYPE_REDIRECT:
fnet_nd6_redirect_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
#if FNET_CFG_MLD
/**************************
* Multicast Listener Query.
**************************/
case FNET_ICMP6_TYPE_MULTICAST_LISTENER_QUERY:
fnet_mld_query_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
#endif
/**************************
* Echo Request.
* RFC4443 4.1: Every node MUST implement an ICMPv6 Echo responder function that
* receives Echo Requests and originates corresponding Echo Replies.
**************************/
case FNET_ICMP6_TYPE_ECHO_REQ:
hdr->type = FNET_ICMP6_TYPE_ECHO_REPLY;
/* RFC4443: the source address of the reply MUST be a unicast
* address belonging to the interface on which
* the Echo Request message was received.*/
if(FNET_IP6_ADDR_IS_MULTICAST(dest_ip))
dest_ip = FNET_NULL;
fnet_icmp6_output(netif, dest_ip/*ipsrc*/, src_ip/*ipdest*/, 0, nb);
fnet_netbuf_free_chain(ip6_nb);
break;
#if FNET_CFG_IP6_PMTU_DISCOVERY
/**************************
* Packet Too Big Message.
**************************/
case FNET_ICMP6_TYPE_PACKET_TOOBIG:
if(netif->pmtu) /* If PMTU is enabled for the interface.*/
{
unsigned long pmtu;
fnet_icmp6_err_header_t *icmp6_err = nb->data_ptr;
/* The header must reside in contiguous area of memory. */
if((tmp_nb = fnet_netbuf_pullup(nb, sizeof(fnet_icmp6_err_header_t))) == 0)
{
goto DISCARD;
}
nb = tmp_nb;
/* RFC 1981.Upon receipt of such a
* message, the source node reduces its assumed PMTU for the path based
* on the MTU of the constricting hop as reported in the Packet Too Big
* message.*/
pmtu = fnet_ntohl(icmp6_err->data);
/* A node MUST NOT increase its estimate of the Path MTU in response to
* the contents of a Packet Too Big message. */
if(netif->pmtu > pmtu)
{
fnet_netif_set_pmtu(netif, pmtu);
}
}
goto DISCARD;
#endif
/**************************
* Destination Unreachable.
**************************/
case FNET_ICMP6_TYPE_DEST_UNREACH:
switch(hdr->code)
{
case FNET_ICMP6_CODE_DU_NO_ROUTE: /* No route to destination. */
case FNET_ICMP6_CODE_DU_BEYOND_SCOPE: /* Beyond scope of source address.*/
prot_cmd = FNET_PROT_NOTIFY_UNREACH_NET;
break;
case FNET_ICMP6_CODE_DU_ADMIN_PROHIBITED: /* Communication with destination administratively prohibited. */
case FNET_ICMP6_CODE_DU_ADDR_UNREACH: /* Address unreachable.*/
prot_cmd = FNET_PROT_NOTIFY_UNREACH_HOST;
break;
case FNET_ICMP6_CODE_DU_PORT_UNREACH: /* Port unreachable.*/
prot_cmd = FNET_PROT_NOTIFY_UNREACH_PORT;
break;
default:
goto DISCARD;
}
/* Protocol notification.*/
{
fnet_ip6_header_t *ip_header;
fnet_prot_if_t *protocol;
unsigned int hdr_err_length = sizeof(fnet_icmp6_err_header_t) + sizeof(fnet_ip6_header_t);
unsigned int hdr_err_data_length = hdr_err_length + 8; /* 8 bytes is enough for transport protocol (port numbers).*/
if(nb->total_length < hdr_err_data_length)
{
goto DISCARD;
}
if(nb->total_length > hdr_err_data_length)
{
fnet_netbuf_trim(&nb, (int)(hdr_err_data_length - nb->total_length));
}
if((tmp_nb = fnet_netbuf_pullup(nb, (int)nb->total_length)) == 0) /* The header must reside in contiguous area of memory.*/
{
goto DISCARD;
}
nb = tmp_nb;
ip_header = (fnet_ip6_header_t *)((char *)nb->data_ptr + sizeof(fnet_icmp6_err_header_t));
if((protocol = fnet_prot_find(AF_INET6, SOCK_UNSPEC, ip_header->next_header)) != 0)
{
if(protocol->prot_control_input)
{
struct sockaddr err_src_addr;
struct sockaddr err_dest_addr;
/* Prepare addreses for upper protocol.*/
fnet_ip6_set_socket_addr(netif, ip_header, &err_src_addr, &err_dest_addr );
fnet_netbuf_trim(&nb, (int)(hdr_err_length)); /* Cut the ICMP error header.*/
protocol->prot_control_input(prot_cmd, &err_src_addr, &err_dest_addr, nb);
}
}
}
goto DISCARD;
default:
goto DISCARD;
}
}
else
{
DISCARD:
fnet_netbuf_free_chain(ip6_nb);
fnet_netbuf_free_chain(nb);
}
}
/************************************************************************
* NAME: fnet_igmp_input
*
* DESCRIPTION: IGMP input function.
*************************************************************************/
static void fnet_igmp_input( fnet_netif_t *netif, fnet_ip4_addr_t src_ip, fnet_ip4_addr_t dest_ip, fnet_netbuf_t *nb, fnet_netbuf_t *ip4_nb)
{
fnet_igmp_header_t *hdr;
fnet_netbuf_t *tmp_nb;
int i;
FNET_COMP_UNUSED_ARG(dest_ip);
FNET_COMP_UNUSED_ARG(src_ip);
fnet_netbuf_free_chain(ip4_nb);
if((netif != 0) && (nb != 0) )
{
/* The header must reside in contiguous area of memory. */
if((tmp_nb = fnet_netbuf_pullup(nb, sizeof(fnet_igmp_header_t))) == 0)
{
goto DISCARD;
}
nb = tmp_nb;
hdr = nb->data_ptr;
/* RFC2236 To be valid, the Query message
* must be at least 8 octets long, have a correct IGMP
* checksum.
*/
if(fnet_checksum(nb, (int)nb->total_length) )
{
goto DISCARD;
}
fnet_igmp_trace("RX", hdr);
/**************************
* IGMP QUERY Processing
**************************/
if(hdr->type == IGMP_HEADER_TYPE_QUERY)
{
/* RFC2236: The group address in the IGMP header must either be zero (a General
* Query) or a valid multicast group address (a Group-Specific Query).
* A General Query applies to all memberships on the interface from
* which the Query is received. A Group-Specific Query applies to
* membership in a single group on the interface from which the Query
* is received. Queries are ignored for memberships in the Non-Member
* state.
*/
if(hdr->group_addr == 0)
/* General Query */
{
/* Find all joined-groups for this interface.*/
for(i=0; i < FNET_CFG_MULTICAST_MAX; i++)
{
if((fnet_ip_multicast_list[i].user_counter > 0) && (fnet_ip_multicast_list[i].netif == netif))
{
/* Send report.*/
fnet_igmp_join(netif, fnet_ip_multicast_list[i].group_addr );
}
}
}
#if FNET_CFG_IGMP_VERSION == 2
else if(FNET_IP4_ADDR_IS_MULTICAST(hdr->group_addr))
/* A Group-Specific Query.*/
{
/* Find specific group.*/
for(i=0; i < FNET_CFG_MULTICAST_MAX; i++)
{
if((fnet_ip_multicast_list[i].user_counter > 0) && (fnet_ip_multicast_list[i].netif == netif) && (fnet_ip_multicast_list[i].group_addr == hdr->group_addr))
{
/* Send report.*/
fnet_igmp_join(netif, fnet_ip_multicast_list[i].group_addr );
break;
}
}
}
#endif /* FNET_CFG_IGMP_VERSION */
/* esle wrong */
}
/************************
* Ignore others
************************/
}
DISCARD:
fnet_netbuf_free_chain(nb);
}
/************************************************************************
* NAME: fnet_icmp6_input
*
* DESCRIPTION: ICMPv6 input function.
*************************************************************************/
static void fnet_icmp6_input(fnet_netif_t *netif, fnet_ip6_addr_t *src_ip, fnet_ip6_addr_t *dest_ip, fnet_netbuf_t *nb, fnet_netbuf_t *ip6_nb)
{
fnet_icmp6_header_t *hdr;
fnet_netbuf_t *tmp_nb;
unsigned short sum;
if((netif != 0) && (nb != 0))
{
/* The header must reside in contiguous area of memory. */
if((tmp_nb = fnet_netbuf_pullup(nb, sizeof(fnet_icmp6_header_t))) == 0)
{
goto DISCARD;
}
nb = tmp_nb;
hdr = nb->data_ptr;
/* Drop Multicast loopback.*/
#if FNET_CFG_LOOPBACK
if ((netif == FNET_LOOP_IF) && FNET_IP6_ADDR_IS_MULTICAST(dest_ip))
{
goto DISCARD;
}
#endif /* FNET_CFG_LOOPBACK */
/* Verify the checksum. */
sum = fnet_checksum_pseudo_start( nb, FNET_HTONS((unsigned short)FNET_IP_PROTOCOL_ICMP6), (unsigned short)nb->total_length );
sum = fnet_checksum_pseudo_end( sum, (char *)src_ip, (char *)dest_ip, sizeof(fnet_ip6_addr_t) );
if(sum)
goto DISCARD;
/************************************************************
* Process incoming ICMPv6 packets.
*************************************************************/
switch (hdr->type)
{
/**************************
* Neighbor Solicitation.
**************************/
case FNET_ICMP6_TYPE_NEIGHBOR_SOLICITATION:
fnet_nd6_neighbor_solicitation_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
/**************************
* Neighbor Advertisemnt.
**************************/
case FNET_ICMP6_TYPE_NEIGHBOR_ADVERTISEMENT:
fnet_nd6_neighbor_advertisement_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
/**************************
* Router Advertisemnt.
**************************/
case FNET_ICMP6_TYPE_ROUTER_ADVERTISEMENT:
fnet_nd6_router_advertisement_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
/**************************
* Router Advertisemnt.
**************************/
case FNET_ICMP6_TYPE_REDIRECT:
fnet_nd6_redirect_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
#if FNET_CFG_MLD
/**************************
* Multicast Listener Query.
**************************/
case FNET_ICMP6_TYPE_MULTICAST_LISTENER_QUERY:
fnet_mld_query_receive(netif, src_ip, dest_ip, nb, ip6_nb);
break;
#endif
/**************************
* Echo Request.
* RFC4443 4.1: Every node MUST implement an ICMPv6 Echo responder function that
* receives Echo Requests and originates corresponding Echo Replies.
**************************/
case FNET_ICMP6_TYPE_ECHO_REQ:
hdr->type = FNET_ICMP6_TYPE_ECHO_REPLY;
/* RFC4443: the source address of the reply MUST be a unicast
* address belonging to the interface on which
* the Echo Request message was received.*/
if(FNET_IP6_ADDR_IS_MULTICAST(dest_ip))
dest_ip = FNET_NULL;
fnet_icmp6_output(netif, dest_ip/*ipsrc*/, src_ip/*ipdest*/, 0, nb); //MD
fnet_netbuf_free_chain(ip6_nb);
break;
#if FNET_CFG_IP6_PMTU_DISCOVERY
/**************************
* Packet Too Big Message.
**************************/
case FNET_ICMP6_TYPE_PACKET_TOOBIG:
if(netif->pmtu) /* If PMTU is enabled for the interface.*/
{
unsigned long pmtu;
fnet_icmp6_err_header_t *icmp6_err = nb->data_ptr;
/* The header must reside in contiguous area of memory. */
if((tmp_nb = fnet_netbuf_pullup(nb, sizeof(fnet_icmp6_err_header_t))) == 0)
{
goto DISCARD;
}
nb = tmp_nb;
/* RFC 1981.Upon receipt of such a
* message, the source node reduces its assumed PMTU for the path based
* on the MTU of the constricting hop as reported in the Packet Too Big
* message.*/
pmtu = fnet_ntohl(icmp6_err->data);
/* A node MUST NOT increase its estimate of the Path MTU in response to
* the contents of a Packet Too Big message. */
if(netif->pmtu > pmtu)
{
fnet_netif_set_pmtu(netif, pmtu);
}
}
goto DISCARD;
#endif
default:
goto DISCARD;
}
}
else
{
DISCARD:
fnet_netbuf_free_chain(ip6_nb);
fnet_netbuf_free_chain(nb);
}
}
/************************************************************************
* NAME: fnet_udp_input
*
* DESCRIPTION: UDP input function.
*************************************************************************/
static void fnet_udp_input(fnet_netif_t *netif, struct sockaddr *foreign_addr, struct sockaddr *local_addr, fnet_netbuf_t *nb, fnet_netbuf_t *ip_nb)
{
fnet_udp_header_t *udp_header = (fnet_udp_header_t *)nb->data_ptr;
fnet_socket_if_t *sock;
fnet_socket_if_t *last;
fnet_size_t udp_length;
fnet_netbuf_t *nb_tmp;
if((netif != 0) && (nb != 0))
{
/* The header must reside in contiguous area of memory.*/
if(fnet_netbuf_pullup(&nb, sizeof(fnet_udp_header_t)) == FNET_ERR)
{
goto BAD;
}
udp_length = fnet_ntohs(udp_header->length);
if(nb->total_length >= udp_length) /* Check the amount of data.*/
{
if(nb->total_length > udp_length)
{
/* Logical size and the physical size of the packet should be the same.*/
fnet_netbuf_trim(&nb, (fnet_int32_t)(udp_length - nb->total_length));
}
#if FNET_CFG_UDP_CHECKSUM
if((udp_header->checksum != 0u)
#if FNET_CFG_CPU_ETH_HW_RX_PROTOCOL_CHECKSUM || FNET_CFG_CPU_ETH_HW_TX_PROTOCOL_CHECKSUM
&& ((nb->flags & FNET_NETBUF_FLAG_HW_PROTOCOL_CHECKSUM) == 0)
#endif
)
{
fnet_uint16_t sum;
sum = fnet_checksum_pseudo_start( nb, FNET_HTONS((fnet_uint16_t)FNET_IP_PROTOCOL_UDP), (fnet_uint16_t)udp_length );
sum = fnet_checksum_pseudo_end( sum, &foreign_addr->sa_data[0], &local_addr->sa_data[0],
(fnet_size_t)((local_addr->sa_family == AF_INET) ? sizeof(fnet_ip4_addr_t) : sizeof(fnet_ip6_addr_t)));
if(sum)
{
goto BAD;
}
}
#endif
fnet_udp_trace("RX", udp_header); /* Trace UDP header.*/
local_addr->sa_port = udp_header->destination_port;
foreign_addr->sa_port = udp_header->source_port;
fnet_netbuf_trim(&nb, (fnet_int32_t)sizeof(fnet_udp_header_t));
/* Demultiplex broadcast & multicast datagrams.*/
if((fnet_socket_addr_is_broadcast(local_addr, netif)) || (fnet_socket_addr_is_multicast(local_addr)))
{
last = 0;
for (sock = fnet_udp_prot_if.head; sock != 0; sock = sock->next)
{
/* Compare local port number.*/
if(sock->local_addr.sa_port != local_addr->sa_port)
{
continue; /* => ignore.*/
}
#if FNET_CFG_MULTICAST
if(fnet_socket_addr_is_multicast(local_addr))
{
fnet_index_t m;
fnet_bool_t for_us = FNET_FALSE;
#if FNET_CFG_IP4
if(local_addr->sa_family == AF_INET)
{
for(m = 0u; m < FNET_CFG_MULTICAST_SOCKET_MAX; m++)
{
if(sock->ip4_multicast_entry[m])
{
if((sock->ip4_multicast_entry[m]->group_addr == ((struct sockaddr_in *)(local_addr))->sin_addr.s_addr) && (sock->ip4_multicast_entry[m]->netif == netif ))
{
for_us = FNET_TRUE;
}
}
}
}
else
#endif
#if FNET_CFG_IP6
if(local_addr->sa_family == AF_INET6)
{
for(m=0u; m < FNET_CFG_MULTICAST_SOCKET_MAX; m++)
{
if(sock->ip6_multicast_entry[m])
{
if(FNET_IP6_ADDR_EQUAL(&sock->ip6_multicast_entry[m]->group_addr, &((struct sockaddr_in6 *)(local_addr))->sin6_addr.s6_addr) && (sock->ip6_multicast_entry[m]->netif == netif))
{
for_us = FNET_TRUE;
}
}
}
}
else
#endif
{}
if(for_us == FNET_FALSE)
{
continue;
}
}
else
#endif /* FNET_CFG_MULTICAST */
{
/* Compare local address.*/
if(!fnet_socket_addr_is_unspecified(&sock->local_addr))
{
if(!fnet_socket_addr_are_equal(&sock->local_addr, local_addr))
{
continue;
}
}
/* Compare foreign address and port number.*/
if(!fnet_socket_addr_is_unspecified(&sock->foreign_addr))
{
if((!fnet_socket_addr_are_equal(&sock->foreign_addr, foreign_addr)) || (sock->foreign_addr.sa_port != foreign_addr->sa_port))
{
continue;
}
}
}
if((last != 0) && (last->receive_buffer.is_shutdown == FNET_FALSE))
{
if((nb_tmp = fnet_netbuf_copy(nb, 0u, FNET_NETBUF_COPYALL, FNET_FALSE)) != 0)
{
if(fnet_socket_buffer_append_address(&(last->receive_buffer), nb_tmp, foreign_addr) == FNET_ERR)
{
fnet_netbuf_free_chain(nb_tmp);
}
}
}
last = sock;
}
if(last == 0)
{
goto BAD;
}
if(last->receive_buffer.is_shutdown) /* Is shutdown.*/
{
goto BAD;
}
if(fnet_socket_buffer_append_address(&(last->receive_buffer), nb, foreign_addr) == FNET_ERR)
{
goto BAD;
}
fnet_netbuf_free_chain(ip_nb);
}
else /* For unicast datagram.*/
{
sock = fnet_socket_lookup(fnet_udp_prot_if.head, local_addr, foreign_addr, FNET_IP_PROTOCOL_UDP);
if(sock)
{
if(sock->receive_buffer.is_shutdown) /* Is shutdown.*/
{
goto BAD;
}
if(fnet_socket_buffer_append_address(&(sock->receive_buffer), nb, foreign_addr) == FNET_ERR)
{
goto BAD;
}
fnet_netbuf_free_chain(ip_nb);
}
else
{
fnet_netbuf_free_chain(nb); /* No match was found, send ICMP destination port unreachable.*/
#if FNET_CFG_IP4
if(local_addr->sa_family == AF_INET)
{
fnet_icmp_error(netif, FNET_ICMP_UNREACHABLE, FNET_ICMP_UNREACHABLE_PORT, ip_nb);
}else
#endif
#if FNET_CFG_IP6
if(local_addr->sa_family == AF_INET6)
{
fnet_icmp6_error(netif, FNET_ICMP6_TYPE_DEST_UNREACH, FNET_ICMP6_CODE_DU_PORT_UNREACH, 0u, ip_nb );
}else
#endif
{}
}
}
}
else
{
goto BAD;
}
}
else
{
BAD:
fnet_netbuf_free_chain(ip_nb);
fnet_netbuf_free_chain(nb);
}
}
