/************************************************************************
* NAME: fnet_arp_request
*
* DESCRIPTION: Sends ARP request.
*************************************************************************/
void fnet_arp_request( fnet_netif_t *netif, fnet_ip4_addr_t ipaddr )
{
fnet_arp_header_t *arp_hdr;
fnet_mac_addr_t sender_addr;
fnet_netbuf_t *nb;
if((nb = fnet_netbuf_new(sizeof(fnet_arp_header_t), FNET_TRUE)) != 0)
{
arp_hdr = nb->data_ptr;
arp_hdr->hard_type = FNET_HTONS(FNET_ARP_HARD_TYPE); /* The type of hardware address (=1 for Ethernet).*/
arp_hdr->prot_type = FNET_HTONS(FNET_ETH_TYPE_IP4); /* The type of protocol address (=0x0800 for IP). */
arp_hdr->hard_size = FNET_ARP_HARD_SIZE; /* The size in bytes of the hardware address (=6). */
arp_hdr->prot_size = FNET_ARP_PROT_SIZE; /* The size in bytes of the protocol address (=4). */
arp_hdr->op = FNET_HTONS(FNET_ARP_OP_REQUEST); /* Opcode. */
fnet_netif_get_hw_addr(netif, sender_addr, sizeof(fnet_mac_addr_t));
fnet_memcpy(arp_hdr->target_hard_addr, fnet_eth_null_addr, sizeof(fnet_mac_addr_t));
fnet_memcpy(arp_hdr->sender_hard_addr, sender_addr, sizeof(fnet_mac_addr_t));
arp_hdr->targer_prot_addr = ipaddr; /* Protocol address of target of this packet.*/
arp_hdr->sender_prot_addr = netif->ip4_addr.address; /* Protocol address of sender of this packet.*/
fnet_arp_trace("TX", arp_hdr); /* Print ARP header. */
((fnet_eth_if_t *)(netif->if_ptr))->output(netif, FNET_ETH_TYPE_ARP, fnet_eth_broadcast, nb);
}
}
/************************************************************************
* NAME: fnet_netif_ip6_addr_autoconf_set
*
* DESCRIPTION: Overwrite the last 64 bits with the interface ID.
* "addr" contains prefix to form an address
*************************************************************************/
int fnet_netif_set_ip6_addr_autoconf(fnet_netif_t *netif, fnet_ip6_addr_t *ip_addr) //OK
{
int result;
unsigned char hw_addr[8];
result = fnet_netif_get_hw_addr(netif, hw_addr, 8 );
if(result == FNET_OK)
{
/* Build Interface identifier.*/
/* Set the 8 last bytes of the IP address based on the Layer 2 identifier.*/
switch(netif->api->hw_addr_size)
{
case 6: /* IEEE 48-bit MAC addresses. */
fnet_memcpy(&(ip_addr->addr[8]), hw_addr, 3);
ip_addr->addr[11] = 0xff;
ip_addr->addr[12] = 0xfe;
fnet_memcpy(&(ip_addr->addr[13]), &hw_addr[3], 3);
ip_addr->addr[8] ^= 0x02;
break;
case 8: /* IEEE EUI-64 identifier.*/
fnet_memcpy(&(ip_addr->addr[8]), hw_addr, 8);
ip_addr->addr[8] ^= 0x02;
break;
/* TBD for others.*/
default:
result = FNET_ERR;
break;
}
}
return result;
}
/************************************************************************
* NAME: fnet_arp_get_mac
*
* DESCRIPTION: Gets MAC address of valid ARP cache entry.
*************************************************************************/
fnet_bool_t fnet_arp_get_mac( fnet_netif_desc_t netif_desc, fnet_ip4_addr_t ip_addr, fnet_mac_addr_t mac_addr)
{
fnet_netif_t *netif = (fnet_netif_t *)netif_desc;
fnet_arp_if_t *arpif;
fnet_mac_addr_t *macaddr_p;
fnet_bool_t result = FNET_FALSE;
if(netif)
{
arpif = netif->arp_if_ptr;
if(arpif)
{
fnet_isr_lock();
macaddr_p = fnet_arp_lookup(netif, ip_addr);
if(macaddr_p)
{
if(mac_addr)
{
fnet_memcpy (mac_addr, *macaddr_p, sizeof(fnet_mac_addr_t));
}
result = FNET_TRUE;
}
fnet_isr_unlock();
}
}
return result;
}
/************************************************************************
* NAME: fnet_fs_rom_fread
*
* DESCRIPTION:
*************************************************************************/
static fnet_size_t fnet_fs_rom_fread (struct fnet_fs_desc *file, void *buf_p, fnet_size_t bytes)
{
fnet_uint8_t *buf = (fnet_uint8_t *)buf_p;
fnet_size_t result = 0u;
struct fnet_fs_rom_node *current;
fnet_size_t size;
fnet_uint32_t pos;
if(file && (file->id) && (file->pos != (fnet_uint32_t)FNET_FS_EOF) && buf)
{
current = (struct fnet_fs_rom_node *)(file->id);
if(current && (current->data_size) && (current->data))
{
size = current->data_size;
pos = file->pos;
if((pos + bytes) > size)
{
bytes = size - pos;
file->pos = (fnet_uint32_t)FNET_FS_EOF;
}
else
{
file->pos += bytes;
}
fnet_memcpy( buf, ¤t->data[pos], bytes );
result = bytes;
}
}
return result;
}
/************************************************************************
* NAME: fapp_http_string_buffer_respond
*
* DESCRIPTION:
*************************************************************************/
static unsigned long fapp_http_string_buffer_respond(char * buffer, unsigned long buffer_size, char * eof, long *cookie)
{
unsigned long result = 0;
char *string_buffer_ptr = (char *) *cookie;
unsigned long send_size = fnet_strlen(string_buffer_ptr);
*eof = 1; /* No aditional send by default. */
if(buffer && buffer_size)
{
if(send_size>buffer_size)
send_size = buffer_size;
fnet_memcpy(buffer, string_buffer_ptr, send_size);
string_buffer_ptr += send_size;
if(*string_buffer_ptr!='\0') /* If any data for sending.*/
*eof = 0; /* Need more itteration. */
result = send_size;
*cookie = (long)string_buffer_ptr; /* Save cgi_buffer_ptr as cookie.*/
}
return result;
}
void fnet_eth_output_ip4(fnet_netif_t *netif, fnet_ip4_addr_t dest_ip_addr, fnet_netbuf_t* nb)
{
fnet_mac_addr_t destination_addr; /* 48-bit destination address */
fnet_mac_addr_t * dest_ptr;
/* Construct Ethernet header. Start with looking up deciding which
* MAC address to use as a destination address. Broadcasts and
* multicasts are special, all other addresses are looked up in the
* ARP table. */
if(fnet_ip_addr_is_broadcast (dest_ip_addr, netif))
{
fnet_memcpy (destination_addr, fnet_eth_broadcast, sizeof(fnet_mac_addr_t));
}
else if(FNET_IP4_ADDR_IS_MULTICAST(dest_ip_addr))
{
/* Hash IP multicast address to MAC address. */
destination_addr[0] = 0x01;
destination_addr[1] = 0x0;
destination_addr[2] = 0x5e;
destination_addr[3] = (unsigned char)(FNET_IP4_ADDR2(dest_ip_addr)& 0x7f);
destination_addr[4] = (unsigned char)(FNET_IP4_ADDR3(dest_ip_addr));
destination_addr[5] = (unsigned char)(FNET_IP4_ADDR4(dest_ip_addr));
//TBD Use macro
}
else
/* Unicast address. */
{
if((dest_ptr = fnet_arp_lookup(netif, dest_ip_addr))!=0)
{
fnet_memcpy (destination_addr, *dest_ptr, sizeof(fnet_mac_addr_t));
}
else
{
fnet_arp_resolve(netif, dest_ip_addr, nb);
goto EXIT;
}
}
/* Send Ethernet frame. */
((fnet_eth_if_t *)(netif->if_ptr))->output(netif, FNET_ETH_TYPE_IP4, destination_addr, nb);
EXIT:
return;
}
/************************************************************************
* NAME: fnet_udp_connect
*
* DESCRIPTION: UDP connect function.
*************************************************************************/
static fnet_return_t fnet_udp_connect( fnet_socket_if_t *sk, struct sockaddr *foreign_addr)
{
fnet_isr_lock();
fnet_memcpy(&sk->foreign_addr, foreign_addr, sizeof(sk->foreign_addr));
sk->state = SS_CONNECTED;
fnet_socket_buffer_release(&sk->receive_buffer);
fnet_isr_unlock();
return (FNET_OK);
}
/************************************************************************
* NAME: fnet_inet_pton_ip4
*
* DESCRIPTION:The function converts from presentation format (string)
* to IPv4 address.
*************************************************************************/
static int fnet_inet_pton_ip4( const char *str, fnet_ip4_addr_t *addr)
{
int is_digit = 0;
int octets = 0;
int ch;
unsigned char tmp[sizeof(*addr)];
unsigned char *tp;
unsigned int val;
*(tp = tmp) = 0;
while ((ch = *str++) != '\0')
{
/* If "c" is digit. */
if((ch >= '0') && (ch <= '9'))
{
val = (unsigned int)(*tp * 10 + (ch - '0'));
if (is_digit && *tp == 0)
goto ERROR;
if (val > 255)
goto ERROR;
*tp = (unsigned char)val;
if (!is_digit)
{
if (++octets > 4)
goto ERROR;
is_digit = 1;
}
}
else if (ch == '.' && is_digit)
{
if (octets == 4)
goto ERROR;
*++tp = 0;
is_digit = 0;
}
else
goto ERROR;
}
if (octets < 4)
goto ERROR;
fnet_memcpy(addr, tmp, sizeof(*addr));
return (FNET_OK);
ERROR:
return (FNET_ERR);
}
/************************************************************************
* NAME: fnet_netbuf_from_buf
*
* DESCRIPTION: Creates a new net_buf and fills it by a content of
* the external data buffer.
*************************************************************************/
fnet_netbuf_t *fnet_netbuf_from_buf( void *data_ptr, int len, int drain )
{
fnet_netbuf_t *nb;
nb = fnet_netbuf_new(len, drain);
if(nb)
fnet_memcpy(nb->data_ptr, data_ptr, (unsigned int)len);
return (nb);
}
/************************************************************************
* NAME: fnet_netbuf_from_buf
*
* DESCRIPTION: Creates a new net_buf and fills it by a content of
* the external data buffer.
*************************************************************************/
fnet_netbuf_t *fnet_netbuf_from_buf( void *data_ptr, fnet_size_t len, fnet_bool_t drain )
{
fnet_netbuf_t *nb;
nb = fnet_netbuf_new(len, drain);
if(nb)
{
fnet_memcpy(nb->data_ptr, data_ptr, len);
}
return (nb);
}
/************************************************************************
* NAME: fapp_http_post_receive
*
* DESCRIPTION:
*************************************************************************/
static int fapp_http_post_receive (char *buffer, unsigned long buffer_size, long *cookie)
{
long post_buffer_index = *cookie;
long post_buffer_free_size = FAPP_HTTP_POST_BUFFER_SIZE - post_buffer_index;
long receive_size = (long)buffer_size;
if(post_buffer_free_size)
{
if(receive_size > post_buffer_free_size)
receive_size = post_buffer_free_size;
fnet_memcpy(&fapp_http_post_buffer[post_buffer_index], buffer, (unsigned)receive_size);
post_buffer_index += receive_size;
fapp_http_post_buffer[post_buffer_index] = '\0';
*cookie = post_buffer_index; /* Save buffer index as cookie.*/
}
return FNET_OK;
}
/************************************************************************
* NAME: fnet_arp_update_entry
*
* DESCRIPTION: Upates ARP table.
*************************************************************************/
static fnet_arp_entry_t *fnet_arp_update_entry(fnet_netif_t *netif, fnet_ip4_addr_t ipaddr, fnet_mac_addr_t ethaddr)
{
fnet_arp_if_t *arpif = netif->arp_if_ptr;
fnet_index_t i;
/* Find an entry to update. */
for (i = 0U; i < FNET_CFG_ARP_TABLE_SIZE; ++i)
{
/* Check if the source IP address of the incoming packet matches
* the IP address in this ARP table entry.*/
if (ipaddr == arpif->arp_table[i].prot_addr)
{
/* Update this and return. */
fnet_memcpy(arpif->arp_table[i].hard_addr, ethaddr, sizeof(fnet_mac_addr_t));
arpif->arp_table[i].cr_time = fnet_timer_ticks();
return (&arpif->arp_table[i]);
}
}
return FNET_NULL;
}
void fnet_eth_output_ip4(fnet_netif_t *netif, fnet_ip4_addr_t dest_ip_addr, fnet_netbuf_t *nb)
{
fnet_mac_addr_t destination_addr; /* 48-bit destination address */
/* Construct Ethernet header. Start with looking up deciding which
* MAC address to use as a destination address. Broadcasts and
* multicasts are special, all other addresses are looked up in the
* ARP table. */
if(fnet_ip_addr_is_broadcast (dest_ip_addr, netif))
{
fnet_memcpy (destination_addr, fnet_eth_broadcast, sizeof(fnet_mac_addr_t));
}
else if(FNET_IP4_ADDR_IS_MULTICAST(dest_ip_addr))
{
/* Hash IP multicast address to MAC address. */
destination_addr[0] = 0x01U;
destination_addr[1] = 0x0U;
destination_addr[2] = 0x5eU;
destination_addr[3] = (fnet_uint8_t)(FNET_IP4_ADDR2(dest_ip_addr) & 0x7fU);
destination_addr[4] = (fnet_uint8_t)(FNET_IP4_ADDR3(dest_ip_addr));
destination_addr[5] = (fnet_uint8_t)(FNET_IP4_ADDR4(dest_ip_addr));
/* TBD Use macro. */
}
else
/* Unicast address. */
{
if(fnet_arp_get_mac( (fnet_netif_desc_t) netif, dest_ip_addr, destination_addr) == FNET_FALSE)
{
fnet_arp_resolve(netif, dest_ip_addr, nb);
goto EXIT;
}
}
/* Send Ethernet frame. */
fnet_eth_output(netif, FNET_ETH_TYPE_IP4, destination_addr, nb);
EXIT:
return;
}
/************************************************************************
* NAME: fnet_arp_get_entry
*
* DESCRIPTION: This function Retrieves ARP cache entry of
* the specified network interface.
*************************************************************************/
fnet_bool_t fnet_arp_get_entry ( fnet_netif_desc_t netif_desc, fnet_index_t n, fnet_arp_entry_info_t *entry_info )
{
fnet_netif_t *netif = (fnet_netif_t *)netif_desc;
fnet_bool_t result = FNET_FALSE;
fnet_arp_if_t *arpif;
fnet_index_t i;
if(netif && entry_info)
{
arpif = netif->arp_if_ptr;
if(arpif)
{
for(i = 0u; i < FNET_CFG_ARP_TABLE_SIZE; i++)
{
/* Skip NOT_USED prefixes. */
if(arpif->arp_table[i].prot_addr != 0U)
{
if(n == 0u)
{
fnet_isr_lock();
/* Fill entry.*/
entry_info->ip_addr = arpif->arp_table[i].prot_addr;
fnet_memcpy(entry_info->mac_addr, arpif->arp_table[i].hard_addr, sizeof(fnet_mac_addr_t));
fnet_isr_unlock();
result = FNET_TRUE;
break;
}
n--;
}
}
}
}
return result;
}
void fnet_netbuf_to_buf( fnet_netbuf_t *nb, fnet_size_t offset, fnet_size_t len, void *data_ptr )
{
fnet_uint8_t *u_buf;
fnet_netbuf_t *tmp_nb;
fnet_size_t tot_len = 0u;
fnet_size_t tot_offset = 0u;
fnet_size_t cur_len;
u_buf = (fnet_uint8_t *)data_ptr;
if(len == 0u)
{
return; /* if input params aren't correct. */
}
tmp_nb = nb;
/* This part is similar to the corresponding part in fnet_netbuf_copy */
do
{
tot_len += tmp_nb->length;
tmp_nb = tmp_nb->next;
} while (tmp_nb);
if(len == FNET_NETBUF_COPYALL) /* If buffer should contain info from all net buffers */
{
tot_len -= offset;
len = (fnet_size_t)tot_len;
}
else
{
if(((len + offset) > tot_len) || (tot_len < offset))
{
return; /* if input params aren't correct. */
}
}
tmp_nb = nb;
if(tmp_nb->length > offset)
{
tot_offset = offset;
}
else
{
while((tot_offset += tmp_nb->length) <= offset)
{
tmp_nb = tmp_nb->next;
}
tot_offset = (tmp_nb->length + offset - tot_offset);
}
tot_len = len;
do
{
/* Calculate the quantity of bytes we copy from the current net_buf*/
cur_len = ((tmp_nb->length - tot_offset > tot_len) ? tot_len : tmp_nb->length - tot_offset);
/* and substract from the total quantity of bytes we copy */
tot_len -= cur_len;
fnet_memcpy(u_buf, (fnet_uint8_t *)tmp_nb->data_ptr + tot_offset, (fnet_size_t)cur_len);
tot_offset = 0u; /* offset is only for the first peace of data*/
u_buf = u_buf + cur_len; /* move the pointer for the next copy */
tmp_nb = tmp_nb->next; /* go to the next net_buf in the chain */
} while (tot_len);
}
/************************************************************************
* DESCRIPTION: Initialization of the Telnet server.
*************************************************************************/
fnet_telnet_desc_t fnet_telnet_init( struct fnet_telnet_params *params )
{
struct sockaddr local_addr;
struct fnet_telnet_if *telnet_if = 0;
/* Socket options. */
const struct linger linger_option = {FNET_TRUE, /*l_onoff*/
4 /*l_linger*/
};
const fnet_size_t bufsize_option = FNET_CFG_TELNET_SOCKET_BUF_SIZE;
const fnet_int32_t keepalive_option = 1;
const fnet_int32_t keepcnt_option = FNET_TELNET_TCP_KEEPCNT;
const fnet_int32_t keepintvl_option = FNET_TELNET_TCP_KEEPINTVL;
const fnet_int32_t keepidle_option = FNET_TELNET_TCP_KEEPIDLE;
fnet_index_t i;
if(params == 0 )
{
FNET_DEBUG_TELNET("TELNET: Wrong init parameters.");
goto ERROR_1;
}
/* Try to find free Telnet server descriptor. */
for(i = 0u; i < FNET_CFG_TELNET_MAX; i++)
{
if(telnet_if_list[i].enabled == FNET_FALSE)
{
telnet_if = &telnet_if_list[i];
break;
}
}
/* No free Telnet server descriptor. */
if(telnet_if == 0)
{
FNET_DEBUG_TELNET("TELNET: No free Telnet Server.");
goto ERROR_1;
}
fnet_memcpy(&local_addr, ¶ms->address, sizeof(local_addr));
if(local_addr.sa_port == 0u)
{
local_addr.sa_port = FNET_CFG_TELNET_PORT; /* Aply the default port. */
}
if(local_addr.sa_family == AF_UNSPEC)
{
local_addr.sa_family = AF_SUPPORTED; /* Assign supported families.*/
}
/* Create listen socket */
if((telnet_if->socket_listen = fnet_socket(local_addr.sa_family, SOCK_STREAM, 0u)) == FNET_ERR)
{
FNET_DEBUG_TELNET("TELNET: Socket creation error.");
goto ERROR_1;
}
if(fnet_socket_bind(telnet_if->socket_listen, (struct sockaddr *)(&local_addr), sizeof(local_addr)) == FNET_ERR)
{
FNET_DEBUG_TELNET("TELNET: Socket bind error.");
goto ERROR_2;
}
/* Set socket options. */
if( /* Setup linger option. */
(fnet_socket_setopt (telnet_if->socket_listen, SOL_SOCKET, SO_LINGER, &linger_option, sizeof(linger_option)) == FNET_ERR) ||
/* Set socket buffer size. */
(fnet_socket_setopt(telnet_if->socket_listen, SOL_SOCKET, SO_RCVBUF, &bufsize_option, sizeof(bufsize_option)) == FNET_ERR) ||
(fnet_socket_setopt(telnet_if->socket_listen, SOL_SOCKET, SO_SNDBUF, &bufsize_option, sizeof(bufsize_option)) == FNET_ERR) ||
/* Enable keepalive_option option. */
(fnet_socket_setopt (telnet_if->socket_listen, SOL_SOCKET, SO_KEEPALIVE, &keepalive_option, sizeof(keepalive_option)) == FNET_ERR) ||
/* Keepalive probe retransmit limit. */
(fnet_socket_setopt (telnet_if->socket_listen, IPPROTO_TCP, TCP_KEEPCNT, &keepcnt_option, sizeof(keepcnt_option)) == FNET_ERR) ||
/* Keepalive retransmit interval.*/
(fnet_socket_setopt (telnet_if->socket_listen, IPPROTO_TCP, TCP_KEEPINTVL, &keepintvl_option, sizeof(keepintvl_option)) == FNET_ERR) ||
/* Time between keepalive probes.*/
(fnet_socket_setopt (telnet_if->socket_listen, IPPROTO_TCP, TCP_KEEPIDLE, &keepidle_option, sizeof(keepidle_option)) == FNET_ERR)
)
{
FNET_DEBUG_TELNET("TELNET: Socket setsockopt() error.");
goto ERROR_2;
}
telnet_if->backlog = FNET_CFG_TELNET_SESSION_MAX;
if(fnet_socket_listen(telnet_if->socket_listen, telnet_if->backlog) == FNET_ERR)
{
FNET_DEBUG_TELNET("TELNET: Socket listen error.");
goto ERROR_2;
}
/* Register service. */
telnet_if->service_descriptor = fnet_poll_service_register(fnet_telnet_state_machine, (void *) telnet_if);
if(telnet_if->service_descriptor == 0)
{
FNET_DEBUG_TELNET("TELNET: Service registration error.");
goto ERROR_2;
}
for(i = 0u; i < FNET_CFG_TELNET_SESSION_MAX; i++)
{
struct fnet_telnet_session_if *session = &telnet_if->session[i];
/* Reset buffer pointers. Move it to init state. */
session->tx_buffer_head_index = 0u;
session->rx_buffer_head = session->rx_buffer;
session->rx_buffer_tail = session->rx_buffer;
session->rx_buffer_end = &session->rx_buffer[FNET_TELNET_RX_BUFFER_SIZE];
/* Setup stream. */
session->stream.id = (fnet_index_t)(session);
session->stream.putchar = fnet_telnet_putchar;
session->stream.getchar = fnet_telnet_getchar;
session->stream.flush = fnet_telnet_flush;
/* Init shell. */
session->shell_params.shell = params->shell;
session->shell_params.cmd_line_buffer = session->cmd_line_buffer;
session->shell_params.cmd_line_buffer_size = sizeof(session->cmd_line_buffer);
session->shell_params.stream = &session->stream;
session->shell_params.echo = FNET_CFG_TELNET_SHELL_ECHO ? FNET_TRUE : FNET_FALSE;
session->socket_foreign = FNET_ERR;
session->state = FNET_TELNET_STATE_LISTENING;
}
telnet_if->session_active = FNET_NULL;
telnet_if->enabled = FNET_TRUE;
return (fnet_telnet_desc_t)telnet_if;
ERROR_2:
fnet_socket_close(telnet_if->socket_listen);
ERROR_1:
return 0;
}
/************************************************************************
* NAME: fapp_bench_udp_tx
*
* DESCRIPTION: Start TX TCP Benchmark.
************************************************************************/
static void fapp_bench_udp_tx (struct fapp_bench_tx_params *params)
{
fnet_int32_t send_result;
fnet_char_t ip_str[FNET_IP_ADDR_STR_SIZE];
fnet_index_t i;
fnet_int32_t received;
const struct linger linger_option = {FNET_TRUE, /*l_onoff*/
4 /*l_linger*/
};
const fnet_size_t bufsize_option = FAPP_BENCH_SOCKET_BUF_SIZE;
struct sockaddr foreign_addr;
fnet_bool_t exit_flag = FNET_FALSE;
fnet_int32_t sock_err;
fnet_size_t option_len;
fnet_shell_desc_t desc = params->desc;
fnet_size_t packet_size = params->packet_size;
fnet_index_t cur_packet_number;
fnet_index_t iterations = params->iteration_number;
fnet_address_family_t family = params->foreign_addr.sa_family;
if(packet_size > FAPP_BENCH_BUFFER_SIZE) /* Check max size.*/
packet_size = FAPP_BENCH_BUFFER_SIZE;
fapp_bench.socket_listen = FNET_ERR;
/* ------ Start test.----------- */
fnet_shell_println(desc, FAPP_DELIMITER_STR);
fnet_shell_println(desc, " UDP TX Test" );
fnet_shell_println(desc, FAPP_DELIMITER_STR);
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_S, "Remote IP addr", fnet_inet_ntop(family, params->foreign_addr.sa_data, ip_str, sizeof(ip_str)));
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_D, "Remote Port", fnet_ntohs(params->foreign_addr.sa_port));
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_D, "Message Size", params->packet_size);
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_D, "Num. of messages", params->packet_number);
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_D, "Num. of iterations", params->iteration_number);
fnet_shell_println(desc, FAPP_TOCANCEL_STR);
fnet_shell_println(desc, FAPP_DELIMITER_STR);
while(iterations--)
{
/* Create socket */
if((fapp_bench.socket_foreign = fnet_socket(family, SOCK_DGRAM, 0)) == FNET_ERR)
{
FNET_DEBUG("BENCH: Socket creation error.\n");
iterations = 0;
goto ERROR_1;
}
/* Set Socket options. */
if( /* Setup linger option. */
(fnet_socket_setopt (fapp_bench.socket_foreign, SOL_SOCKET, SO_LINGER, (fnet_uint8_t *)&linger_option, sizeof(linger_option)) == FNET_ERR) ||
/* Set socket buffer size. */
(fnet_socket_setopt(fapp_bench.socket_foreign, SOL_SOCKET, SO_RCVBUF, (fnet_uint8_t *) &bufsize_option, sizeof(bufsize_option))== FNET_ERR) ||
(fnet_socket_setopt(fapp_bench.socket_foreign, SOL_SOCKET, SO_SNDBUF, (fnet_uint8_t *) &bufsize_option, sizeof(bufsize_option))== FNET_ERR)
)
{
FNET_DEBUG("BENCH: Socket setsockopt error.\n");
iterations = 0;
goto ERROR_2;
}
/* Bind to the server.*/
fnet_shell_println(desc,"Connecting.");
fnet_memcpy(&foreign_addr, ¶ms->foreign_addr, sizeof(foreign_addr));
if(fnet_socket_connect(fapp_bench.socket_foreign, &foreign_addr, sizeof(foreign_addr))== FNET_ERR)
{
fnet_shell_println(desc, "Connection failed.");
iterations = 0;
goto ERROR_2;
}
/* Sending.*/
fnet_shell_println(desc,"Sending.");
fapp_bench.bytes = 0;
fapp_bench.remote_bytes = 0;
cur_packet_number = 0;
fapp_bench.first_time = fnet_timer_ticks();
while(1)
{
send_result = fnet_socket_send( fapp_bench.socket_foreign, (fnet_uint8_t*)(&fapp_bench.buffer[0]), packet_size, 0);
fapp_bench.last_time = fnet_timer_ticks();
if ( send_result == FNET_ERR )
{
option_len = sizeof(sock_err);
fnet_socket_getopt(fapp_bench.socket_foreign, SOL_SOCKET, SO_ERROR, (fnet_uint8_t*)&sock_err, &option_len);
fnet_shell_println(desc, "socket_error = %d", sock_err);
iterations = 0;
goto ERROR_2;
}
else
{
fapp_bench.bytes += send_result;
cur_packet_number ++;
exit_flag = fnet_shell_ctrlc (desc); /* Check [Ctrl+c]*/
if((cur_packet_number >= params->packet_number)|| exit_flag)
{
if(exit_flag)
{
fnet_shell_println(desc, FAPP_SHELL_CANCELED_CTRL_C);
iterations = 0;
}
break;/* => TX END. */
}
}
}
/* Send End mark.*/
for(i=0; i < FAPP_BENCH_TX_UDP_END_ITERATIONS; i++)
{
fnet_uint32_t ack_bytes;
/* Send END mark.*/
fnet_socket_send( fapp_bench.socket_foreign, (fnet_uint8_t*)(&fapp_bench.buffer[0]), 1, 0);
fnet_timer_delay(1);
/* Check ACK. It should contain recieved amount of data.*/
received = fnet_socket_recv(fapp_bench.socket_foreign, (fnet_uint8_t*)(&ack_bytes), sizeof(ack_bytes), 0);
if(received == sizeof(ack_bytes)) /* ACK received.*/
{
fapp_bench.remote_bytes = fnet_ntohl(ack_bytes);
break;
}
else if(received == FNET_ERR)
{
break;
}
else
{}
}
/* Print benchmark results.*/
fapp_bench_print_results (desc);
ERROR_2:
fnet_socket_close(fapp_bench.socket_foreign);
}
ERROR_1:
fnet_shell_println(desc, FAPP_BENCH_COMPLETED_STR);
}
/************************************************************************
* NAME: fapp_bench_tcp_tx
*
* DESCRIPTION: Start TX TCP Benchmark.
************************************************************************/
static void fapp_bench_tcp_tx (struct fapp_bench_tx_params *params)
{
fnet_int32_t send_result;
fnet_char_t ip_str[FNET_IP_ADDR_STR_SIZE];
const struct linger linger_option = {FNET_TRUE, /*l_onoff*/
4 /*l_linger*/
};
const fnet_size_t bufsize_option = FAPP_BENCH_SOCKET_BUF_SIZE;
const fnet_int32_t keepalive_option = 1;
const fnet_int32_t keepcnt_option = FAPP_BENCH_TCP_KEEPCNT;
const fnet_int32_t keepintvl_option = FAPP_BENCH_TCP_KEEPINTVL;
const fnet_int32_t keepidle_option = FAPP_BENCH_TCP_KEEPIDLE;
struct sockaddr foreign_addr;
fnet_bool_t exit_flag = FNET_FALSE;
fnet_int32_t sock_err ;
fnet_size_t option_len;
fnet_shell_desc_t desc = params->desc;
fnet_socket_state_t connection_state;
fnet_size_t packet_size = params->packet_size;
fnet_index_t cur_packet_number;
fnet_size_t buffer_offset;
fnet_index_t iterations = params->iteration_number;
fnet_address_family_t family = params->foreign_addr.sa_family;
if(packet_size > FAPP_BENCH_BUFFER_SIZE) /* Check max size.*/
packet_size = FAPP_BENCH_BUFFER_SIZE;
fapp_bench.socket_listen = FNET_ERR;
/* ------ Start test.----------- */
fnet_shell_println(desc, FAPP_DELIMITER_STR);
fnet_shell_println(desc, " TCP TX Test" );
fnet_shell_println(desc, FAPP_DELIMITER_STR);
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_S, "Remote IP Addr", fnet_inet_ntop(family, params->foreign_addr.sa_data, ip_str, sizeof(ip_str)));
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_D, "Remote Port", fnet_ntohs(params->foreign_addr.sa_port));
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_D, "Message Size", params->packet_size);
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_D, "Num. of messages", params->packet_number);
fnet_shell_println(desc, FAPP_SHELL_INFO_FORMAT_D, "Num. of iterations", params->iteration_number);
fnet_shell_println(desc, FAPP_TOCANCEL_STR);
fnet_shell_println(desc, FAPP_DELIMITER_STR);
while(iterations--)
{
/* Create socket */
if((fapp_bench.socket_foreign = fnet_socket(family, SOCK_STREAM, 0)) == FNET_ERR)
{
FNET_DEBUG("BENCH: Socket creation error.\n");
iterations = 0;
goto ERROR_1;
}
/* Set Socket options. */
if( /* Setup linger option. */
(fnet_socket_setopt (fapp_bench.socket_foreign, SOL_SOCKET, SO_LINGER, (fnet_uint8_t *)&linger_option, sizeof(linger_option)) == FNET_ERR) ||
/* Set socket buffer size. */
(fnet_socket_setopt(fapp_bench.socket_foreign, SOL_SOCKET, SO_RCVBUF, (fnet_uint8_t *) &bufsize_option, sizeof(bufsize_option))== FNET_ERR) ||
(fnet_socket_setopt(fapp_bench.socket_foreign, SOL_SOCKET, SO_SNDBUF, (fnet_uint8_t *) &bufsize_option, sizeof(bufsize_option))== FNET_ERR) ||
/* Enable keepalive_option option. */
(fnet_socket_setopt (fapp_bench.socket_foreign, SOL_SOCKET, SO_KEEPALIVE, (fnet_uint8_t *)&keepalive_option, sizeof(keepalive_option)) == FNET_ERR) ||
/* Keepalive probe retransmit limit. */
(fnet_socket_setopt (fapp_bench.socket_foreign, IPPROTO_TCP, TCP_KEEPCNT, (fnet_uint8_t *)&keepcnt_option, sizeof(keepcnt_option)) == FNET_ERR) ||
/* Keepalive retransmit interval.*/
(fnet_socket_setopt (fapp_bench.socket_foreign, IPPROTO_TCP, TCP_KEEPINTVL, (fnet_uint8_t *)&keepintvl_option, sizeof(keepintvl_option)) == FNET_ERR) ||
/* Time between keepalive probes.*/
(fnet_socket_setopt (fapp_bench.socket_foreign, IPPROTO_TCP, TCP_KEEPIDLE, (fnet_uint8_t *)&keepidle_option, sizeof(keepidle_option)) == FNET_ERR)
)
{
FNET_DEBUG("BENCH: Socket setsockopt error.\n");
iterations = 0;
goto ERROR_2;
}
/* Connect to the server.*/
fnet_shell_println(desc,"Connecting.");
fnet_memcpy(&foreign_addr, ¶ms->foreign_addr, sizeof(foreign_addr));
fnet_socket_connect(fapp_bench.socket_foreign, (struct sockaddr *)(&foreign_addr), sizeof(foreign_addr));
do
{
option_len = sizeof(connection_state);
fnet_socket_getopt(fapp_bench.socket_foreign, SOL_SOCKET, SO_STATE, (fnet_uint8_t*)&connection_state, &option_len);
}
while (connection_state == SS_CONNECTING);
if(connection_state != SS_CONNECTED)
{
fnet_shell_println(desc, "Connection failed.");
iterations = 0;
goto ERROR_2;
}
/* Sending.*/
fnet_shell_println(desc,"Sending.");
fapp_bench.bytes = 0;
fapp_bench.remote_bytes = 0;
cur_packet_number = 0;
buffer_offset = 0;
fapp_bench.first_time = fnet_timer_ticks();
while(1)
{
send_result = fnet_socket_send( fapp_bench.socket_foreign, (fnet_uint8_t*)(&fapp_bench.buffer[buffer_offset]), (packet_size-buffer_offset), 0);
fapp_bench.last_time = fnet_timer_ticks();
if ( send_result == FNET_ERR )
{
option_len = sizeof(sock_err);
fnet_socket_getopt(fapp_bench.socket_foreign, SOL_SOCKET, SO_ERROR, &sock_err, &option_len);
fnet_shell_println(desc, "Socket error = %d", sock_err);
iterations = 0;
#if 0
/* Print benchmark results.*/
fapp_bench_print_results (desc);
#endif
break;
}
else if(send_result)
{
fapp_bench.bytes += send_result;
buffer_offset += send_result;
if(buffer_offset == packet_size)
{
cur_packet_number ++;
buffer_offset = 0;
}
exit_flag = fnet_shell_ctrlc(desc); /* Check [Ctrl+c]*/
if((cur_packet_number >= params->packet_number)|| exit_flag)
{
if(exit_flag)
{
fnet_shell_println(desc, FAPP_SHELL_CANCELED_CTRL_C);
iterations = 0;
}
/* Print benchmark results.*/
fapp_bench_print_results (desc);
break;
}
}
else
{}
}
ERROR_2:
fnet_socket_close(fapp_bench.socket_foreign);
}
ERROR_1:
fnet_shell_println(desc, FAPP_BENCH_COMPLETED_STR);
}
/************************************************************************
* 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_netbuf_pullup
*
* DESCRIPTION: Create a data buffer for the first net_buf with the
* length len
*************************************************************************/
fnet_return_t fnet_netbuf_pullup( fnet_netbuf_t **nb_ptr, fnet_size_t len)
{
fnet_netbuf_t *nb = *nb_ptr;
fnet_size_t tot_len = 0u;
fnet_size_t offset;
fnet_netbuf_t *tmp_nb;
fnet_netbuf_t *nb_run;
void *new_buf;
/* Check length*/
if(nb->total_length < len)
{
return FNET_OK;
}
if((nb->length >= len) || (len == 0u) || (nb == 0))
{
/* if function shouldn't do anything*/
return FNET_OK;
}
tmp_nb = nb;
tot_len += tmp_nb->length;
/* search of the last buffer, from which the data have to be copied*/
while((tot_len < len) && tmp_nb)
{
tmp_nb = tmp_nb->next;
tot_len += tmp_nb->length;
}
new_buf = (struct net_buf_data *)fnet_malloc_netbuf((fnet_size_t)len + sizeof(fnet_uint32_t)/* For reference_counter */);
if(new_buf == 0)
{
return FNET_ERR;
}
((fnet_uint32_t *)new_buf)[0] = 1u; /* First element is used by the reference_counter.*/
/* Copy into it the contents of first data buffer. Skip the reference counter (placed in the first bytes). */
fnet_memcpy(&((fnet_uint32_t *)new_buf)[1], nb->data_ptr, nb->length);
offset = nb->length;
/* Free old data buffer (for the first net_buf) */
if(((fnet_uint32_t *)nb->data)[0] == 1u) /* If nobody uses this data buffer. */
{
fnet_free_netbuf(nb->data);
}
else /* Else decrement reference counter */
{
((fnet_uint32_t *)nb->data)[0] = ((fnet_uint32_t *)nb->data)[0] - 1u;
}
/* Currently data buffer contains the contents of the first buffer */
nb->data = &((fnet_uint32_t *)new_buf)[0];
nb->data_ptr = &((fnet_uint32_t *)new_buf)[1];
nb_run = nb->next; /* Let's start from the next buffer */
while(nb_run != tmp_nb) /* Copy full data buffers */
{
fnet_memcpy((fnet_uint8_t *)nb->data_ptr + offset, nb_run->data_ptr, nb_run->length);
if(nb_run != tmp_nb)
{
offset += nb_run->length;
nb_run = fnet_netbuf_free(nb_run);
}
}
tot_len = len - offset;
/* Copy the remaining part and change data pointer and length of the
* last net_buf, which is the source for the first net_buf */
fnet_memcpy((fnet_uint8_t *)nb->data_ptr + offset, nb_run->data_ptr, tot_len);
nb_run->length -= tot_len;
if(nb_run->length == 0u)
{
nb_run = fnet_netbuf_free(nb_run);
}
else
{
nb_run->data_ptr = (fnet_uint8_t *)nb_run->data_ptr + tot_len;
}
/* Setting up the params of the first net_buf.*/
nb->next = nb_run;
nb->length = (fnet_size_t)len;
*nb_ptr = nb;
return FNET_OK;
}
/************************************************************************
* NAME: fnet_mld_send
*
* DESCRIPTION: Sends MLD message defined by type:
* FNET_ICMP6_TYPE_MULTICAST_LISTENER_REPORT or FNET_ICMP6_TYPE_MULTICAST_LISTENER_DONE
*************************************************************************/
static void fnet_mld_send(fnet_netif_t *netif, fnet_ip6_addr_t *group_addr, fnet_uint8_t type)
{
fnet_netbuf_t *nb;
fnet_netbuf_t *nb_option;
fnet_mld_header_t *mld_header;
fnet_mld_ra_option_header_t *ra_option_header;
FNET_COMP_PACKED_VAR fnet_uint16_t *checksum_p;
fnet_ip6_addr_t *ip_src;
fnet_ip6_addr_t *ip_dst;
/* [RFC2710] EXCLUDING the link-scope all-nodes address and any multicast
* addresses of scope 0 (reserved) or 1(node-local).*/
if((FNET_IP6_ADDR_MULTICAST_SCOPE(group_addr) > FNET_IP6_ADDR_SCOPE_INTERFACELOCAL)
&& !FNET_IP6_ADDR_EQUAL(&fnet_ip6_addr_linklocal_allnodes, group_addr))
{
/* Construct Router Alert option + MLD meassage */
if((nb = fnet_netbuf_new(sizeof(fnet_mld_header_t), FNET_FALSE)) != 0)
{
if((nb_option = fnet_netbuf_new(sizeof(fnet_mld_ra_option_header_t), FNET_FALSE)) != 0)
{
/* Fill Hop-by_Hop Options header.*/
ra_option_header = (fnet_mld_ra_option_header_t *)(nb_option->data_ptr);
fnet_memcpy ((void *)ra_option_header, (void *)(&mld_ra_option), sizeof(mld_ra_option));
/* Fill MLD message. */
mld_header = (fnet_mld_header_t *)(nb->data_ptr);
fnet_memset_zero(mld_header, sizeof(fnet_mld_header_t));
mld_header->icmp6_header.type = type;
FNET_IP6_ADDR_COPY(group_addr, &mld_header->multicast_addr);
/* Checksum calculation.*/
mld_header->icmp6_header.checksum = 0;
mld_header->icmp6_header.checksum = fnet_checksum_pseudo_start(nb, FNET_HTONS((fnet_uint16_t)FNET_IP_PROTOCOL_ICMP6), (fnet_uint16_t)nb->total_length);
checksum_p = &mld_header->icmp6_header.checksum;
/* Concatanate Hop-by_Hop Options with MLD header. */
nb = fnet_netbuf_concat(nb_option, nb);
/* Source Address Selection for MLD, by RFC3590.*/
/* [RFC3590] MLD Report and Done messages are sent with a link-local address as
* the IPv6 source address, if a valid address is available on the interface.*/
ip_src = fnet_netif_get_ip6_addr_valid_link_local(netif);
/* [RFC3590] If a valid link-local address is not available (e.g., one has not been configured),
* the message is sent with the unspecified address. */
if(ip_src == FNET_NULL)
{
ip_src = (fnet_ip6_addr_t *)&fnet_ip6_addr_any;
netif->mld_invalid = FNET_TRUE;
}
else
{
netif->mld_invalid = FNET_FALSE;
}
/* When a node ceases to listen to a multicast address on an interface,
* it SHOULD send a single Done message to the link-scope all-routers
* multicast address (FF02::2)*/
if(type == FNET_ICMP6_TYPE_MULTICAST_LISTENER_DONE)
{
ip_dst =(fnet_ip6_addr_t *)&fnet_ip6_addr_linklocal_allrouters;
}
else
{
ip_dst = group_addr;
}
/* Send via IPv6*/
fnet_ip6_output(netif, ip_src, ip_dst, FNET_IP6_TYPE_HOP_BY_HOP_OPTIONS, FNET_MLD_HOP_LIMIT, nb, checksum_p);
}
else
{
fnet_netbuf_free_chain(nb);
}
}
}
}
static int fnet_inet_pton_ip6( const char *str, fnet_ip6_addr_t *addr )
{
const char xdigits_l[] = "0123456789abcdef";
const char xdigits_u[] = "0123456789ABCDEF";
unsigned char tmp[sizeof(*addr)];
unsigned char *tp;
unsigned char *endp;
unsigned char *colonp;
const char *xdigits;
int ch;
int seen_xdigits;
unsigned int val;
fnet_memset_zero((tp = tmp), sizeof(*addr));
endp = tp + sizeof(*addr);
colonp = FNET_NULL;
/* Leading :: */
if (*str == ':')
if (*++str != ':')
goto ERROR;
seen_xdigits = 0;
val = 0;
while (((ch = *str++) != '\0') && (ch != '%'))
{
const char *pch;
if ((pch = fnet_strchr((xdigits = xdigits_l), ch)) == FNET_NULL)
pch = fnet_strchr((xdigits = xdigits_u), ch);
if (pch != FNET_NULL)
{
val <<= 4;
val |= (pch - xdigits);
if (++seen_xdigits > 4)
goto ERROR;
continue;
}
if (ch == ':')
{
if (!seen_xdigits)
{
if (colonp)
goto ERROR;
colonp = tp;
continue;
}
else if (*str == '\0')
{
goto ERROR;
}
else
{}
if (tp + 2 > endp)
goto ERROR;
*tp++ = (unsigned char) ((val >> 8) & 0xff);
*tp++ = (unsigned char) (val & 0xff);
seen_xdigits = 0;
val = 0;
continue;
}
goto ERROR;
}
if (seen_xdigits)
{
if (tp + 2 > endp)
goto ERROR;
*tp++ = (unsigned char) ((val >> 8) & 0xff);
*tp++ = (unsigned char) (val & 0xff);
}
if (colonp != FNET_NULL)
{
/*
* Shift.
*/
const int n = tp - colonp;
int i;
if (tp == endp)
goto ERROR;
for (i = 1; i <= n; i++)
{
endp[- i] = colonp[n - i];
colonp[n - i] = 0;
}
tp = endp;
}
if (tp != endp)
goto ERROR;
fnet_memcpy(addr, tmp, sizeof(*addr));
return (FNET_OK);
ERROR:
return (FNET_ERR);
}
/************************************************************************
* 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_arp_add_entry
*
* DESCRIPTION: Adds entry to the ARP table.
*************************************************************************/
static fnet_arp_entry_t *fnet_arp_add_entry( fnet_netif_t *netif, fnet_ip4_addr_t ipaddr,
const fnet_mac_addr_t ethaddr )
{
fnet_arp_if_t *arpif = &(((fnet_eth_if_t *)(netif->if_ptr))->arp_if);
int i, j;
unsigned long max_time;
/* Find an entry to update. */
for (i = 0; i < FNET_ARP_TABLE_SIZE; ++i)
{
/* Check if the source IP address of the incoming packet matches
* the IP address in this ARP table entry.*/
if(ipaddr == arpif->arp_table[i].prot_addr)
{
/* Update this and return. */
fnet_memcpy(arpif->arp_table[i].hard_addr, ethaddr, sizeof(fnet_mac_addr_t));
arpif->arp_table[i].cr_time = fnet_timer_ticks();
goto ADDED;
}
}
/* If we get here, no existing ARP table entry was found. */
/* Find an unused entry in the ARP table. */
for (i = 0; i < FNET_ARP_TABLE_SIZE; ++i)
{
if(arpif->arp_table[i].prot_addr == 0)
{
break;
}
}
/* If no unused entry is found, we try to find the oldest entry and throw it away.*/
if(i == FNET_ARP_TABLE_SIZE)
{
max_time = 0;
j = 0;
for (i = 0; i < FNET_ARP_TABLE_SIZE; ++i)
{
if((fnet_timer_ticks() - arpif->arp_table[i].cr_time) > max_time)
{
max_time = fnet_timer_ticks() - arpif->arp_table[i].cr_time;
j = i;
}
}
i = j;
}
/* Now, it is the ARP table entry which we will fill with the new information. */
if(arpif->arp_table[i].hold)
{
fnet_netbuf_free_chain(arpif->arp_table[i].hold);
arpif->arp_table[i].hold = 0;
arpif->arp_table[i].hold_time = 0;
}
arpif->arp_table[i].prot_addr = ipaddr;
fnet_memcpy(arpif->arp_table[i].hard_addr, ethaddr, sizeof(fnet_mac_addr_t));
arpif->arp_table[i].cr_time = fnet_timer_ticks();
ADDED:
return ( &arpif->arp_table[i]);
}