/************************************************************************
* NAME: fnet_ping_state_machine
*
* DESCRIPTION: PING service state machine.
************************************************************************/
static void fnet_ping_state_machine(void *fnet_ping_if_p)
{
fnet_int32_t received;
fnet_icmp_echo_header_t *hdr;
fnet_ping_if_t *ping_if = (fnet_ping_if_t *)fnet_ping_if_p;
struct sockaddr addr;
fnet_size_t addr_len = sizeof(addr);
switch(ping_if->state)
{
/*===================================*/
case FNET_PING_STATE_SENDING_REQUEST:
/* Build message.*/
hdr = (fnet_icmp_echo_header_t *)&fnet_ping_if.buffer[0];
/* Fill ICMP Echo request header.*/
fnet_memset_zero(hdr, sizeof(*hdr));
hdr->header.type = (fnet_uint8_t)((fnet_ping_if.family == AF_INET) ? FNET_ICMP_ECHO: FNET_ICMP6_TYPE_ECHO_REQ);
hdr->identifier = FNET_CFG_PING_IDENTIFIER;
fnet_ping_if.sequence_number++;
hdr->sequence_number = fnet_htons(fnet_ping_if.sequence_number);
/* Fill payload data by pattern.*/
fnet_memset(&fnet_ping_if.buffer[sizeof(*hdr)], ping_if->pattern, ping_if->packet_size);
/* Checksum.*/
#if FNET_CFG_IP4
if(ping_if->family == AF_INET)
{
hdr->header.checksum = fnet_checksum_buf(&fnet_ping_if.buffer[0], (sizeof(*hdr) + ping_if->packet_size));
}
else
#endif
#if FNET_CFG_IP6
if(ping_if->family == AF_INET6)
{
const fnet_ip6_addr_t *src_ip = fnet_ip6_select_src_addr(FNET_NULL, (fnet_ip6_addr_t *)ping_if->target_addr.sa_data); /*TBD Check result.*/
hdr->header.checksum = fnet_checksum_pseudo_buf(&fnet_ping_if.buffer[0],
(fnet_uint16_t)(sizeof(*hdr) + ping_if->packet_size),
FNET_HTONS((fnet_uint16_t)IPPROTO_ICMPV6),
(const fnet_uint8_t *)src_ip,
ping_if->target_addr.sa_data,
sizeof(fnet_ip6_addr_t));
}
else
#endif
{}
/* Send request.*/
fnet_socket_sendto(fnet_ping_if.socket_foreign, (fnet_uint8_t*)(&fnet_ping_if.buffer[0]), (sizeof(*hdr) + ping_if->packet_size), 0u, &ping_if->target_addr, sizeof(ping_if->target_addr));
ping_if->packet_count--;
fnet_ping_if.send_time = fnet_timer_ticks();
ping_if->state = FNET_PING_STATE_WAITING_REPLY;
break;
/*===================================*/
case FNET_PING_STATE_WAITING_REPLY:
/* Receive data */
received = fnet_socket_recvfrom(ping_if->socket_foreign, (fnet_uint8_t*)(&ping_if->buffer[0]), FNET_PING_BUFFER_SIZE, 0u, &addr, &addr_len );
if(received > 0 )
{
fnet_uint16_t checksum = 0u;
hdr = (fnet_icmp_echo_header_t *)(ping_if->buffer);
/* Check checksum.*/
#if FNET_CFG_IP4
if(ping_if->family == AF_INET)
{
checksum = fnet_checksum_buf(&fnet_ping_if.buffer[0], (fnet_size_t)received);
}
else
#endif
#if 0 /* #if FNET_CFG_IP6 */ /* TBD case to receive from multicast address ff02::1*/
if(ping_if->family == AF_INET6)
{
checksum = fnet_checksum_pseudo_buf(&fnet_ping_if.buffer[0],
(fnet_uint16_t)(received),
IPPROTO_ICMPV6,
ping_if->local_addr.sa_data,
ping_if->target_addr.sa_data,
sizeof(fnet_ip6_addr_t));
}
else
#endif
{}
/* Check header.*/
if( checksum
||(hdr->header.type != ((addr.sa_family == AF_INET) ? FNET_ICMP_ECHOREPLY: FNET_ICMP6_TYPE_ECHO_REPLY))
||(hdr->identifier != FNET_CFG_PING_IDENTIFIER)
||(hdr->sequence_number != fnet_htons(ping_if->sequence_number)) )
{
goto NO_DATA;
}
/* Call handler.*/
if(ping_if->handler)
{
ping_if->handler(FNET_ERR_OK, ping_if->packet_count, &addr, ping_if->handler_cookie);
}
if(ping_if->packet_count)
{
ping_if->state = FNET_PING_STATE_WAITING_TIMEOUT;
}
else
{
fnet_ping_release();
}
}
else if(received == FNET_ERR)
{
/* Call handler.*/
if(ping_if->handler)
{
fnet_error_t sock_err ;
fnet_size_t option_len;
/* Get socket error.*/
option_len = sizeof(sock_err);
fnet_socket_getopt(ping_if->socket_foreign, SOL_SOCKET, SO_ERROR, (fnet_uint8_t*)&sock_err, &option_len);
ping_if->handler(sock_err, ping_if->packet_count, FNET_NULL, ping_if->handler_cookie);
}
if(ping_if->packet_count)
{
ping_if->state = FNET_PING_STATE_WAITING_TIMEOUT;
}
else
{
fnet_ping_release();
}
}
else /* No data. Check timeout */
{
NO_DATA:
if(fnet_timer_get_interval(fnet_ping_if.send_time, fnet_timer_ticks()) > fnet_ping_if.timeout_clk)
{
/* Call handler.*/
if(ping_if->handler)
{
ping_if->handler(FNET_ERR_TIMEDOUT, ping_if->packet_count, FNET_NULL, ping_if->handler_cookie);
}
if(ping_if->packet_count)
{
ping_if->state = FNET_PING_STATE_SENDING_REQUEST;
}
else
{
fnet_ping_release();
}
}
}
break;
/*===================================*/
case FNET_PING_STATE_WAITING_TIMEOUT:
if(fnet_timer_get_interval(fnet_ping_if.send_time, fnet_timer_ticks()) > fnet_ping_if.timeout_clk)
{
ping_if->state = FNET_PING_STATE_SENDING_REQUEST;
}
break;
default:
break; /* do nothing, avoid compiler warning "enumeration value not handled in switch" */
}
}
/************************************************************************
* 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_udp_output
*
* DESCRIPTION: UDP output function
*************************************************************************/
static fnet_error_t fnet_udp_output( struct sockaddr *src_addr, const struct sockaddr *dest_addr,
fnet_socket_option_t *sockoption, fnet_netbuf_t *nb )
{
fnet_netbuf_t *nb_header;
fnet_udp_header_t *udp_header;
fnet_error_t error = FNET_ERR_OK;
FNET_COMP_PACKED_VAR fnet_uint16_t *checksum_p;
fnet_netif_t *netif = FNET_NULL;
fnet_scope_id_t scope_id = 0u;
/* Check Scope ID.*/
if(dest_addr->sa_scope_id) /* Take scope id from destination address.*/
{
scope_id = dest_addr->sa_scope_id;
}
else /* Take scope id from source address.*/
{
scope_id = src_addr->sa_scope_id;
}
netif = (fnet_netif_t *)fnet_netif_get_by_scope_id(scope_id); /* It can be FNET_NULL, in case scope_id is 0.*/
/* Construct UDP header.*/
if((nb_header = fnet_netbuf_new(sizeof(fnet_udp_header_t), FNET_TRUE)) == 0)
{
fnet_netbuf_free_chain(nb);
return (FNET_ERR_NOMEM);
}
udp_header = (fnet_udp_header_t *)nb_header->data_ptr;
udp_header->source_port = src_addr->sa_port; /* Source port number.*/
udp_header->destination_port = dest_addr->sa_port; /* Destination port number.*/
nb = fnet_netbuf_concat(nb_header, nb);
udp_header->length = fnet_htons((fnet_uint16_t)nb->total_length); /* Length.*/
/* Checksum calculation.*/
udp_header->checksum = 0u;
#if FNET_CFG_UDP_CHECKSUM
#if FNET_CFG_CPU_ETH_HW_TX_IP_CHECKSUM
if( 0
#if FNET_CFG_IP4
||( (dest_addr->sa_family == AF_INET)
&& ((netif = fnet_ip_route(((struct sockaddr_in *)(dest_addr))->sin_addr.s_addr))!= FNET_NULL)
&& (netif->features & FNET_NETIF_FEATURE_HW_TX_PROTOCOL_CHECKSUM)
&& (fnet_ip_will_fragment(netif, nb->total_length) == FNET_FALSE) /* Fragmented packets are not inspected.*/ )
#endif
#if FNET_CFG_IP6
||( (dest_addr->sa_family == AF_INET6)
&& (netif || (((netif = fnet_ip6_route(&((struct sockaddr_in6 *)(src_addr))->sin6_addr.s6_addr, &((struct sockaddr_in6 *)(dest_addr))->sin6_addr.s6_addr)))!= FNET_NULL) )
&& (netif->features & FNET_NETIF_FEATURE_HW_TX_PROTOCOL_CHECKSUM)
&& (fnet_ip6_will_fragment(netif, nb->total_length) == FNET_FALSE) /* Fragmented packets are not inspected.*/ )
#endif
)
{
nb->flags |= FNET_NETBUF_FLAG_HW_PROTOCOL_CHECKSUM;
checksum_p = 0;
}
else
#endif /* FNET_CFG_CPU_ETH_HW_TX_IP_CHECKSUM */
{
udp_header->checksum = fnet_checksum_pseudo_start( nb, FNET_HTONS((fnet_uint16_t)FNET_IP_PROTOCOL_UDP), (fnet_uint16_t)nb->total_length );
checksum_p = &udp_header->checksum;
}
#endif /* FNET_CFG_UDP_CHECKSUM */
#if FNET_CFG_IP4
if(dest_addr->sa_family == AF_INET)
{
error = fnet_ip_output(netif,
((const struct sockaddr_in *)(src_addr))->sin_addr.s_addr,
((const struct sockaddr_in *)(dest_addr))->sin_addr.s_addr,
FNET_IP_PROTOCOL_UDP, sockoption->ip_opt.tos,
#if FNET_CFG_MULTICAST
(fnet_uint8_t)((FNET_IP4_ADDR_IS_MULTICAST(((const struct sockaddr_in *)(dest_addr))->sin_addr.s_addr)?sockoption->ip_opt.ttl_multicast:sockoption->ip_opt.ttl)),
#else
sockoption->ip_opt.ttl,
#endif /* FNET_CFG_MULTICAST */
nb, FNET_UDP_DF, sockoption->so_dontroute,
checksum_p
);
}
else
#endif
#if FNET_CFG_IP6
if(dest_addr->sa_family == AF_INET6)
{
error = fnet_ip6_output( netif,
fnet_socket_addr_is_unspecified(src_addr)? FNET_NULL : &((struct sockaddr_in6 *)(src_addr))->sin6_addr.s6_addr,
&((const struct sockaddr_in6 *)(dest_addr))->sin6_addr.s6_addr,
FNET_IP_PROTOCOL_UDP,
FNET_IP6_ADDR_IS_MULTICAST(&((const struct sockaddr_in6 *)(dest_addr))->sin6_addr.s6_addr)?sockoption->ip6_opt.hops_multicast:sockoption->ip6_opt.hops_unicast,
nb,
checksum_p
);
}
else
#endif
{}
return (error);
}
