/************************************************************************
* 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_error
*
* DESCRIPTION: Sends ICMP error message.
*************************************************************************/
void fnet_icmp_error( fnet_netif_t *netif, fnet_uint8_t type, fnet_uint8_t code, fnet_netbuf_t *nb )
{
fnet_ip_header_t *ipheader;
fnet_netbuf_t *nb_header;
fnet_icmp_err_header_t *icmpheader;
fnet_ip4_addr_t source_addr;
fnet_ip4_addr_t destination_addr;
if(nb)
{
ipheader = (fnet_ip_header_t *)nb->data_ptr;
source_addr = ipheader->source_addr;
destination_addr = ipheader->desination_addr;
/* Do not send error if not the first fragment of message (RFC1122)*/
if((FNET_IP_HEADER_GET_OFFSET(ipheader) != 0u) ||
/* Do not send error on ICMP error messages*/
((ipheader->protocol == FNET_IP_PROTOCOL_ICMP)
&& (!FNET_ICMP_IS_QUERY_TYPE(((fnet_icmp_header_t *)((fnet_uint8_t *)(nb->data_ptr) + (FNET_IP_HEADER_GET_HEADER_LENGTH(ipheader) << 2)))->type)))
/* Do not send error on a datagram whose source address does not define a single
* host -- e.g., a zero address, a loopback address, a
* broadcast address, a multicast address, or a Class E
* address.*/
|| (fnet_ip_addr_is_broadcast(source_addr, netif))
|| FNET_IP4_ADDR_IS_MULTICAST(source_addr)
|| FNET_IP4_CLASS_E(source_addr)
/* Do not send error on a datagram destined to an IP broadcast or IP multicast address*/
|| (fnet_ip_addr_is_broadcast(destination_addr, netif))
|| FNET_IP4_ADDR_IS_MULTICAST(destination_addr)
/* Do not send error on datagram sent as a link-layer broadcast or multicast.*/
||((nb->flags & FNET_NETBUF_FLAG_BROADCAST) != 0u) || ((nb->flags & FNET_NETBUF_FLAG_MULTICAST) != 0u)
)
{
goto FREE_NB;
}
/* Construct ICMP error header*/
if((nb_header = fnet_netbuf_new((sizeof(fnet_icmp_err_header_t) - sizeof(fnet_ip_header_t)), FNET_FALSE)) == 0)
{
goto FREE_NB;
}
icmpheader = (fnet_icmp_err_header_t *)nb_header->data_ptr;
icmpheader->fields.unused = 0u;
if(type == FNET_ICMP_PARAMPROB)
{
icmpheader->fields.ptr = fnet_htons((fnet_uint16_t)code);
code = 0u;
}
else if((type == FNET_ICMP_PARAMPROB) && (code == FNET_ICMP_UNREACHABLE_NEEDFRAG) && netif)
{
icmpheader->fields.mtu = fnet_htons((fnet_uint16_t)netif->mtu);
}
else
{}
icmpheader->header.type = type;
icmpheader->header.code = code;
if((fnet_size_t)((FNET_IP_HEADER_GET_HEADER_LENGTH(ipheader) << 2) + 8u) < nb->total_length)
{
fnet_netbuf_trim(&nb, (fnet_int32_t)((fnet_size_t)((FNET_IP_HEADER_GET_HEADER_LENGTH(ipheader) << 2) + 8u) - nb->total_length));
}
nb = fnet_netbuf_concat(nb_header, nb);
fnet_icmp_output(netif, destination_addr, source_addr, nb);
return;
FREE_NB:
fnet_netbuf_free_chain(nb);
}
}
/************************************************************************
* NAME: fnet_netbuf_cut_center
*
* DESCRIPTION: Cuts len bytes in net_buf queue starting from offset "offset"
*
*************************************************************************/
fnet_netbuf_t *fnet_netbuf_cut_center( fnet_netbuf_t ** nb_ptr, fnet_size_t offset, fnet_size_t len )
{
fnet_netbuf_t *head_nb, *tmp_nb, *nb;
fnet_size_t tot_len;
if(len == 0u)
{
return (0);
}
nb = (fnet_netbuf_t *) *nb_ptr;
if((nb->total_length < (len + offset)) || (nb == 0))
{
return (0);
}
if(offset == 0u) /* The first case - when we cut from the begin of buffer.*/
{
fnet_netbuf_trim(&nb, (fnet_int32_t)len);
*nb_ptr = nb;
return (nb);
}
head_nb = nb;
tmp_nb = nb;
tot_len = nb->length;
while((nb != 0) && (offset >= tot_len))
{
nb = nb->next; /* Run up th the first net_buf, which points */
tot_len += nb->length; /* to the data, which should be erased.*/
if((nb != 0) && (offset >= tot_len))
{
tmp_nb = nb; /* To store previous pointer. */
}
}
if(tot_len - nb->length == offset) /* If we start cut from the begin of buffer. */
{
nb->total_length = head_nb->total_length; /* For correct fnet_netbuf_trim execution.*/
fnet_netbuf_trim(&tmp_nb->next, (fnet_int32_t)len);
head_nb->total_length -= len;
return ((fnet_netbuf_t *) *nb_ptr);
}
/* If only the middle of one net_buf should be cut.*/
if(tot_len - offset > len)
{
head_nb->total_length -= len;
/* Split one net_uf into two.*/
if(((fnet_uint32_t *)nb->data)[0] == 1u) /* If we can simply erase them (reference_counter == 1).*/
{
fnet_memcpy((fnet_uint8_t *)nb->data_ptr + nb->length - tot_len + offset,
(fnet_uint8_t *)nb->data_ptr + nb->length - tot_len + offset + len,
(fnet_size_t)(tot_len - offset - len));
nb->length -= len;
}
else
{
head_nb = fnet_netbuf_new(tot_len - offset - len, FNET_FALSE);
if(head_nb == 0) /* If no free memory.*/
{
nb = (fnet_netbuf_t *) *nb_ptr;
nb->total_length += len;
return (0);
}
fnet_memcpy(head_nb->data_ptr,
(fnet_uint8_t *)nb->data_ptr + nb->length - tot_len + offset + len,
(fnet_size_t)(tot_len - offset - len));
head_nb->next = nb->next;
nb->next = head_nb;
nb->length -= tot_len - offset;
}
return ((fnet_netbuf_t *) *nb_ptr);
}
if(tot_len - offset == len) /* If we cut from the middle of buffer to the end only.*/
{
nb->length -= len;
head_nb->total_length -= len;
return ((fnet_netbuf_t *) *nb_ptr);
}
else /* Cut from the middle of net_buf to the end and trim remaining info*/
{ /* (tot_len-offset < len)*/
nb->length -= tot_len - offset;
nb->next->total_length = head_nb->total_length; /* For correct fnet_netbuf_trim execution. */
fnet_netbuf_trim(&nb->next, (fnet_int32_t)(len - (tot_len - offset)));
head_nb->total_length -= len;
return ((fnet_netbuf_t *) *nb_ptr);
}
}
/************************************************************************
* 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_icmp6_error
*
* DESCRIPTION: Sends ICMPv6 error message.
*************************************************************************/
void fnet_icmp6_error( fnet_netif_t *netif, unsigned char type, unsigned char code, unsigned long param, fnet_netbuf_t *origin_nb )
{
fnet_ip6_header_t *ip6_header;
fnet_icmp6_err_header_t *icmp6_err_header;
fnet_ip6_addr_t *src_ip;
const fnet_ip6_addr_t *dest_ip;
fnet_netbuf_t *nb_header;
if(origin_nb)
{
/* Limit to FNET_IP6_DEFAULT_MTU. */
if(origin_nb->total_length > (FNET_IP6_DEFAULT_MTU - (sizeof(fnet_icmp6_err_header_t) + sizeof(fnet_ip6_header_t)) ))
fnet_netbuf_trim(&origin_nb, (int)(FNET_IP6_DEFAULT_MTU - (sizeof(fnet_icmp6_err_header_t) + sizeof(fnet_ip6_header_t)) - origin_nb->total_length));
ip6_header = origin_nb->data_ptr;
src_ip = &ip6_header->source_addr;
dest_ip = &ip6_header->destination_addr;
/*******************************************************************
* RFC 4443:
* (e) An ICMPv6 error message MUST NOT be originated as a result of
* receiving the following:
*******************************************************************/
/* (e.1) An ICMPv6 error message. */
/* (e.2) An ICMPv6 REDIRECT message [IPv6-DISC].*/
if (ip6_header->next_header == FNET_IP_PROTOCOL_ICMP6) /* TBD Extension header case.*/
{
/* Make sure the packet has at least a 'TYPE' field */
if (ip6_header->length == 0)
{
goto FREE_NB;
}
icmp6_err_header = (fnet_icmp6_err_header_t *)((unsigned char *)ip6_header + sizeof(fnet_ip6_header_t));
if (FNET_ICMP6_TYPE_IS_ERROR(icmp6_err_header->icmp6_header.type) || (icmp6_err_header->icmp6_header.type == FNET_ICMP6_TYPE_REDIRECT ) )
{
goto FREE_NB;
}
}
/*
* (e.3) A packet destined to an IPv6 multicast address. (There are
* two exceptions to this rule: (1) the Packet Too Big Message
* (Section 3.2) to allow Path MTU discovery to work for IPv6
* multicast, and (2) the Parameter Problem Message, Code 2
* (Section 3.4) reporting an unrecognized IPv6 option (see
* Section 4.2 of [IPv6]) that has the Option Type highestorder
* two bits set to 10).
* (e.4) A packet sent as a link-layer multicast (the exceptions
* from e.3 apply to this case, too).
*/
if(FNET_IP6_ADDR_IS_MULTICAST(dest_ip)
&& !( (type == FNET_ICMP6_TYPE_PACKET_TOOBIG)
|| ((type == FNET_ICMP6_TYPE_PARAM_PROB) && (code == FNET_ICMP6_CODE_PP_OPTION))) )
{
goto FREE_NB;
}
else
{
if(FNET_IP6_ADDR_IS_MULTICAST(dest_ip))
{
/* We may not use multicast address as source. Get real source address. */
dest_ip = fnet_ip6_select_src_addr(netif, src_ip /*dest*/);
}
}
/*
* (e.5) A packet sent as a link-layer broadcast (the exceptions
* from e.3 apply to this case, too). TBD
*/
/*
* (e.6) A packet whose source address does not uniquely identify a
* single node -- e.g., the IPv6 Unspecified Address, an IPv6
* multicast address, or an address known by the ICMP message
* originator to be an IPv6 anycast address.
*/
if(FNET_IP6_ADDR_IS_MULTICAST(src_ip) || FNET_IP6_ADDR_EQUAL(&fnet_ip6_addr_any, src_ip))
{
goto FREE_NB;
}
/* Construct ICMPv6 error header.*/
if((nb_header = fnet_netbuf_new((sizeof(fnet_icmp6_err_header_t)), FNET_FALSE)) == 0)
goto FREE_NB;
icmp6_err_header = nb_header->data_ptr;
icmp6_err_header->icmp6_header.type = type;
icmp6_err_header->icmp6_header.code = code;
icmp6_err_header->data = fnet_htonl(param);
origin_nb = fnet_netbuf_concat(nb_header, origin_nb);
fnet_icmp6_output( netif, dest_ip/*ipsrc*/, src_ip/*ipdest*/, 0, origin_nb);
return;
FREE_NB:
fnet_netbuf_free_chain(origin_nb);
}
}
/************************************************************************
* NAME: fnet_arp_input
*
* DESCRIPTION: ARP input function.
*************************************************************************/
void fnet_arp_input( fnet_netif_t *netif, fnet_netbuf_t *nb )
{
fnet_arp_if_t *arpif = &(((fnet_eth_if_t *)(netif->if_ptr))->arp_if);
fnet_arp_header_t *arp_hdr = nb->data_ptr;
fnet_mac_addr_t local_addr;
fnet_arp_entry_t *entry;
if(!((nb == 0) /* The packet is wrong. */
|| (nb->total_length < sizeof(fnet_arp_header_t)) || (arp_hdr->hard_type != FNET_HTONS(FNET_ARP_HARD_TYPE))
|| (arp_hdr->hard_size != FNET_ARP_HARD_SIZE) || (arp_hdr->prot_type != FNET_HTONS(FNET_ETH_TYPE_IP4))
|| (arp_hdr->prot_size != FNET_ARP_PROT_SIZE)))
{
if(nb->total_length > sizeof(fnet_arp_header_t))
{
/* Logical size and the physical size of the packet should be the same.*/
fnet_netbuf_trim(&nb, (int)(sizeof(fnet_arp_header_t) - nb->total_length));
}
fnet_arp_trace("RX", arp_hdr); /* Print ARP header. */
fnet_netif_get_hw_addr(netif, local_addr, sizeof(fnet_mac_addr_t));
if(!(!fnet_memcmp(arp_hdr->sender_hard_addr, local_addr, sizeof(fnet_mac_addr_t)) /* It's from me => ignore it.*/
|| !fnet_memcmp(arp_hdr->sender_hard_addr, fnet_eth_broadcast, sizeof(fnet_mac_addr_t))) /* It's broadcast=> error. */
)
{
fnet_ip4_addr_t sender_prot_addr = arp_hdr->sender_prot_addr;
fnet_ip4_addr_t targer_prot_addr = arp_hdr->targer_prot_addr;
if(sender_prot_addr != netif->ip4_addr.address) /* Check Duplicate IP address.*/
{
if(targer_prot_addr == netif->ip4_addr.address) /* It's for me.*/
{
entry = fnet_arp_add_entry(netif, sender_prot_addr, arp_hdr->sender_hard_addr);
}
else
{
entry = fnet_arp_update_entry(netif, sender_prot_addr, arp_hdr->sender_hard_addr);
}
if(entry && entry->hold)
{
/* Send waiting data.*/
((fnet_eth_if_t *)(netif->if_ptr))->output(netif, FNET_ETH_TYPE_IP4, entry->hard_addr, entry->hold);
entry->hold = 0;
entry->hold_time = 0;
}
}
else
{
/* IP is duplicated. */
fnet_event_raise(arpif->arp_event);
}
/* ARP request. If it asked for our address, we send out a reply.*/
if((arp_hdr->op == FNET_HTONS(FNET_ARP_OP_REQUEST)) && (targer_prot_addr == netif->ip4_addr.address))
{
arp_hdr->op = FNET_HTONS(FNET_ARP_OP_REPLY); /* Opcode */
fnet_memcpy(arp_hdr->target_hard_addr, arp_hdr->sender_hard_addr, sizeof(fnet_mac_addr_t));
fnet_memcpy(arp_hdr->sender_hard_addr, local_addr, sizeof(fnet_mac_addr_t));
arp_hdr->targer_prot_addr = arp_hdr->sender_prot_addr;
arp_hdr->sender_prot_addr = netif->ip4_addr.address;
fnet_arp_trace("TX Reply", arp_hdr); /* Print ARP header. */
((fnet_eth_if_t *)(netif->if_ptr))->output(netif, FNET_ETH_TYPE_ARP, fnet_eth_broadcast, nb);
return;
}
}
}
fnet_netbuf_free_chain(nb);
}
/************************************************************************
* NAME: fnet_icmp_input
*
* DESCRIPTION: ICMP input function.
*************************************************************************/
static void fnet_icmp_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_icmp_header_t *hdr;
fnet_icmp_err_header_t *hdr_err;
fnet_prot_notify_t prot_cmd;
fnet_prot_if_t *protocol;
fnet_netbuf_t *tmp_nb;
fnet_netbuf_free_chain(ip4_nb);
if((netif != 0) && (nb != 0) )
{
if((tmp_nb = fnet_netbuf_pullup(nb, sizeof(fnet_icmp_header_t))) == 0) /* The header must reside in contiguous area of memory. */
{
goto DISCARD;
}
nb = tmp_nb;
hdr = nb->data_ptr;
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, (int)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((tmp_nb = fnet_netbuf_pullup(nb, sizeof(fnet_icmp_timestamp_header_t))) == 0)
{
goto DISCARD;
}
nb = tmp_nb;
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((tmp_nb = fnet_netbuf_pullup(nb, sizeof(fnet_icmp_mask_header_t))) == 0)
{
goto DISCARD;
}
nb = tmp_nb;
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;
}
goto NOTIFY_PROT;
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;
}
goto NOTIFY_PROT;
case FNET_ICMP_PARAMPROB: /* Parameter Problem Message.*/
if(hdr->code > 1)
goto DISCARD;
prot_cmd = FNET_PROT_NOTIFY_PARAMPROB;
goto NOTIFY_PROT;
case FNET_ICMP_SOURCEQUENCH: /* Source Quench Message; packet lost, slow down.*/
if(hdr->code)
goto DISCARD;
prot_cmd = FNET_PROT_NOTIFY_QUENCH;
NOTIFY_PROT: /* Protocol notification.*/
if(nb->total_length < (sizeof(fnet_icmp_err_header_t) + 8))
goto DISCARD;
if(nb->total_length > (sizeof(fnet_icmp_err_header_t) + 8))
fnet_netbuf_trim(&nb, (int)((sizeof(fnet_icmp_err_header_t) + 8) - 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;
hdr_err = (fnet_icmp_err_header_t *)nb->data_ptr;
if((protocol = fnet_prot_find(AF_INET, SOCK_UNSPEC, hdr_err->ip.protocol)) != 0)
{
if(protocol->prot_control_input)
protocol->prot_control_input(prot_cmd, (void *)(&hdr_err->ip));
}
goto DISCARD;
/************************
* 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_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);
}
}
