/************************************************************************ * NAME: fnet_raw_input * * DESCRIPTION: RAW input function. *************************************************************************/ void fnet_raw_input(fnet_netif_t *netif, struct sockaddr *foreign_addr, struct sockaddr *local_addr, fnet_netbuf_t *nb, fnet_netbuf_t *ip_nb) { fnet_socket_t *sock; fnet_socket_t *last; fnet_netbuf_t *nb_tmp; int protocol_number; if(netif && nb && nb->total_length) { #if FNET_CFG_IP4 if(foreign_addr->sa_family == AF_INET) { protocol_number = ((fnet_ip_header_t *)(ip_nb->data_ptr))->protocol; } else #endif #if FNET_CFG_IP6 if(foreign_addr->sa_family == AF_INET6) { protocol_number = ((fnet_ip6_header_t *)(ip_nb->data_ptr))->next_header; } else #endif {protocol_number = 0;} /* 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_raw_prot_if.head; sock != 0; sock = sock->next) { /* Ignore local port number.*/ /* Check protocol number.*/ if(sock->protocol_number != protocol_number) continue; /* => ignore.*/ #if FNET_CFG_MULTICAST if(fnet_socket_addr_is_multicast(local_addr)) { int m; int for_us = FNET_FALSE; #if FNET_CFG_IP4 if(local_addr->sa_family == AF_INET) { for(m=0; 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; } } } #endif #if FNET_CFG_IP6 if(local_addr->sa_family == AF_INET6) { for(m=0; 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; } } } #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)) ) continue; } } if((last != 0) && (last->receive_buffer.is_shutdown == 0)) { if((nb_tmp = fnet_netbuf_copy(nb, 0, FNET_NETBUF_COPYALL, 0)) != 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; /* Copy buffer.*/ if((nb_tmp = fnet_netbuf_copy(nb, 0, FNET_NETBUF_COPYALL, 0)) != 0) { if(fnet_socket_buffer_append_address(&(last->receive_buffer), nb_tmp, foreign_addr) == FNET_ERR) { fnet_netbuf_free_chain(nb_tmp); goto BAD; } } else goto BAD; } else /* For unicast datagram.*/ { sock = fnet_socket_lookup(fnet_raw_prot_if.head, local_addr, foreign_addr, protocol_number); if(sock) { if(sock->receive_buffer.is_shutdown) /* Is shutdown.*/ goto BAD; /* Copy buffer.*/ if((nb_tmp = fnet_netbuf_copy(nb, 0, FNET_NETBUF_COPYALL, 0)) != 0) { if(fnet_socket_buffer_append_address(&(sock->receive_buffer), nb_tmp, foreign_addr) == FNET_ERR) { fnet_netbuf_free_chain(nb_tmp); goto BAD; } } else goto BAD; } } } BAD: return; }
/************************************************************************ * 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); } }