/************************************************************************
* 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: fapp_bench_udp_rx
*
* DESCRIPTION: Start Benchmark UDP server.
************************************************************************/
static void fapp_bench_udp_rx (fnet_shell_desc_t desc, fnet_address_family_t family, struct sockaddr *multicast_address /* optional, set to 0*/)
{
struct sockaddr local_addr;
const unsigned long bufsize_option = FAPP_BENCH_SOCKET_BUF_SIZE;
int received;
char ip_str[FNET_IP_ADDR_STR_SIZE];
struct sockaddr addr;
unsigned int addr_len;
int is_first = 1;
int exit_flag = 0;
/* Create listen socket */
if((fapp_bench.socket_listen = socket(family, SOCK_DGRAM, 0)) == SOCKET_INVALID)
{
FNET_DEBUG("BENCH: Socket creation error.\n");
goto ERROR_1;
}
/*Bind.*/
fnet_memset_zero(&local_addr, sizeof(local_addr));
local_addr.sa_port = FAPP_BENCH_PORT;
local_addr.sa_family = family;
if(bind(fapp_bench.socket_listen, &local_addr, sizeof(local_addr)) == SOCKET_ERROR)
{
FNET_DEBUG("BENCH: Socket bind error.\n");
goto ERROR_2;
}
/* Set socket options. */
if(
/* Set socket buffer size. */
(setsockopt(fapp_bench.socket_listen, SOL_SOCKET, SO_RCVBUF, (char *) &bufsize_option, sizeof(bufsize_option))== SOCKET_ERROR) ||
(setsockopt(fapp_bench.socket_listen, SOL_SOCKET, SO_SNDBUF, (char *) &bufsize_option, sizeof(bufsize_option))== SOCKET_ERROR)
)
{
FNET_DEBUG("BENCH: Socket setsockopt error.\n");
goto ERROR_2;
}
/* Join multicast group, if set. */
if(multicast_address)
{
#if FNET_CFG_IP4
if(multicast_address->sa_family == AF_INET)
{
struct ip_mreq mreq; /* Multicast group information.*/
mreq.imr_multiaddr.s_addr = ((struct sockaddr_in*)multicast_address)->sin_addr.s_addr;
mreq.imr_interface.s_addr = FNET_HTONL(INADDR_ANY); /* Default Interface.*/
/* Join multicast group. */
if(setsockopt(fapp_bench.socket_listen, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&mreq, sizeof(mreq)) == SOCKET_ERROR)
{
FNET_DEBUG("BENCH: Joining to multicast group is failed.\n");
goto ERROR_2;
}
}
#endif
#if FNET_CFG_IP6
if(multicast_address->sa_family == AF_INET6)
{
struct ipv6_mreq mreq6; /* Multicast group information.*/
FNET_IP6_ADDR_COPY(&((struct sockaddr_in6*)multicast_address)->sin6_addr.s6_addr, &mreq6.ipv6imr_multiaddr.s6_addr);
mreq6.ipv6imr_interface = ((struct sockaddr_in6*)multicast_address)->sin6_scope_id;
/* Join multicast group. */
if(setsockopt(fapp_bench.socket_listen, IPPROTO_IPV6, IPV6_JOIN_GROUP, (char *)&mreq6, sizeof(mreq6)) == SOCKET_ERROR)
{
FNET_DEBUG("BENCH: Joining to multicast group is failed.\n");
goto ERROR_2;
}
}
#endif
}
/* ------ Start test.----------- */
fnet_shell_println(desc, FAPP_DELIMITER_STR);
fnet_shell_println(desc, " UDP RX Test" );
fnet_shell_println(desc, FAPP_DELIMITER_STR);
fapp_netif_addr_print(desc, family, fapp_default_netif, FNET_FALSE);
if(multicast_address)
{
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_S, "Multicast Group", fnet_inet_ntop(multicast_address->sa_family, (char*)(multicast_address->sa_data), ip_str, sizeof(ip_str)) );
}
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_D, "Local Port", FNET_NTOHS(FAPP_BENCH_PORT));
fnet_shell_println(desc, FAPP_TOCANCEL_STR);
fnet_shell_println(desc, FAPP_DELIMITER_STR);
while(exit_flag == 0) /* Main loop */
{
fnet_shell_println(desc, "Waiting.");
fapp_bench.bytes = 0;
fapp_bench.remote_bytes = 0;
addr_len = sizeof(addr);
is_first = 1;
while(exit_flag == 0) /* Test loop. */
{
/* Receive data */
received = recvfrom (fapp_bench.socket_listen, (char*)(&fapp_bench.buffer[0]), FAPP_BENCH_BUFFER_SIZE, 0,
&addr, &addr_len );
if(received >= FAPP_BENCH_UDP_END_BUFFER_LENGTH)
{
/* Reset timeout. */
fapp_bench.last_time = fnet_timer_ticks();
if(is_first)
{
if( received > FAPP_BENCH_UDP_END_BUFFER_LENGTH )
{
fnet_shell_println(desc,"Receiving from %s:%d", fnet_inet_ntop(addr.sa_family, (char*)(addr.sa_data), ip_str, sizeof(ip_str)), fnet_ntohs(addr.sa_port));
fapp_bench.first_time = fnet_timer_ticks();
is_first = 0;
}
}
else
{
if(received == FAPP_BENCH_UDP_END_BUFFER_LENGTH ) /* End of transfer. */
{
/* Send ACK containing amount of received data.*/
unsigned long ack_bytes = fnet_htonl(fapp_bench.bytes);
/* Send several times, just to be sure that it is received/not lost.*/
sendto(fapp_bench.socket_listen, (char*)(&ack_bytes), sizeof(ack_bytes), 0, (struct sockaddr*)&addr, sizeof(addr));
sendto(fapp_bench.socket_listen, (char*)(&ack_bytes), sizeof(ack_bytes), 0, (struct sockaddr*)&addr, sizeof(addr));
sendto(fapp_bench.socket_listen, (char*)(&ack_bytes), sizeof(ack_bytes), 0, (struct sockaddr*)&addr, sizeof(addr));
/* Print benchmark results.*/
fapp_bench_print_results (desc);
break;
}
else
fapp_bench.bytes += received;
}
}
else
{
/* Check error. Check timeout */
if(received == SOCKET_ERROR)
{
fnet_shell_println(desc, "BENCH: Error (%d).", fnet_error_get());
break;
}
/* Check timeout. */
if((is_first == 0) &&
(fnet_timer_get_interval(fapp_bench.last_time, fnet_timer_ticks()) > (FAPP_UDP_TIMEOUT_MS/FNET_TIMER_PERIOD_MS)))
{
fnet_shell_println(desc, "BENCH: Exit on timeout.");
fapp_bench_print_results (desc);
break;
}
}
exit_flag = fnet_shell_ctrlc (desc);
}
}
ERROR_2:
closesocket(fapp_bench.socket_listen);
ERROR_1:
fnet_shell_println(desc, FAPP_BENCH_COMPLETED_STR);
}
/************************************************************************
* 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);
}
}
