sock_result_t socket_receivefrom(sock_handle_t sd, void* buffer, socklen_t bufLen, uint32_t flags, sockaddr_t* addr, socklen_t* addrsize)
{
socket_t* socket = from_handle(sd);
volatile wiced_result_t result = WICED_INVALID_SOCKET;
uint16_t read_len = 0;
if (is_open(socket) && is_udp(socket)) {
std::lock_guard<socket_t> lk(*socket);
wiced_packet_t* packet = NULL;
// UDP receive timeout changed to 0 sec so as not to block
if ((result=wiced_udp_receive(udp(socket), &packet, WICED_NO_WAIT))==WICED_SUCCESS) {
wiced_ip_address_t wiced_ip_addr;
uint16_t port;
if ((result=wiced_udp_packet_get_info(packet, &wiced_ip_addr, &port))==WICED_SUCCESS) {
uint32_t ipv4 = GET_IPV4_ADDRESS(wiced_ip_addr);
addr->sa_data[0] = (port>>8) & 0xFF;
addr->sa_data[1] = port & 0xFF;
addr->sa_data[2] = (ipv4 >> 24) & 0xFF;
addr->sa_data[3] = (ipv4 >> 16) & 0xFF;
addr->sa_data[4] = (ipv4 >> 8) & 0xFF;
addr->sa_data[5] = ipv4 & 0xFF;
result=read_packet(packet, (uint8_t*)buffer, bufLen, &read_len);
}
inline void setAddress(wiced_ip_address_t* addr, HAL_IPAddress& target) {
target.ipv4 = GET_IPV4_ADDRESS(*addr);
}
void application_start( )
{
int i;
wiced_result_t status;
wiced_mac_t my_mac_address;
uint32_t my_ip_address;
wiced_ip_address_t ipv4_address;
wiced_keep_alive_packet_t keep_alive_packet_info;
/* Initialize the device */
wiced_init( );
/* Bring up the network on the STA interface */
wiced_network_up( WICED_STA_INTERFACE, WICED_USE_EXTERNAL_DHCP_SERVER, NULL );
/* Setup the ARP keep alive packet to copy in my MAC & IP address */
wiced_wifi_get_mac_address( &my_mac_address );
wiced_ip_get_ipv4_address( WICED_STA_INTERFACE, &ipv4_address );
my_ip_address = htonl( GET_IPV4_ADDRESS(ipv4_address) );
memcpy( &arp_packet[ 6 ], &my_mac_address, 6 );
memcpy( &arp_packet[ 22 ], &my_mac_address, 6 );
memcpy( &arp_packet[ 28 ], &my_ip_address, 4 );
memcpy( &arp_packet[ 38 ], &my_ip_address, 4 );
/* Turn off print buffers, so print output occurs immediately */
setvbuf( stdout, NULL, _IONBF, 0 );
/* Setup a Null function data frame keep alive */
keep_alive_packet_info.keep_alive_id = KEEP_ALIVE_ID_NFD,
keep_alive_packet_info.period_msec = KEEP_ALIVE_PERIOD_NFD_MSEC,
keep_alive_packet_info.packet_length = 0;
status = wiced_wifi_add_keep_alive( &keep_alive_packet_info );
switch ( status )
{
case WICED_SUCCESS:
{
WPRINT_APP_INFO( ( "\r\nAdded:\n") );
WPRINT_APP_INFO( ( " - Null Function Data frame with repeat period of %d milliseconds \n", KEEP_ALIVE_PERIOD_NFD_MSEC ) );
break;
}
case WICED_TIMEOUT:
{
WPRINT_APP_INFO( ( "Timeout: Adding Null Function Data frame keep alive packet\n" ) );
break;
}
default:
WPRINT_APP_INFO( ( "Error[%d]: Adding Null Function Data frame keep alive packet\n", status ) );
break;
}
/* Setup an ARP packet keep alive */
keep_alive_packet_info.keep_alive_id = KEEP_ALIVE_ID_ARP,
keep_alive_packet_info.period_msec = KEEP_ALIVE_PERIOD_ARP_MSEC,
keep_alive_packet_info.packet_length = sizeof(arp_packet)-1;
keep_alive_packet_info.packet = (uint8_t*)arp_packet;
status = wiced_wifi_add_keep_alive( &keep_alive_packet_info );
switch ( status )
{
case WICED_SUCCESS:
{
WPRINT_APP_INFO( ( " - ARP packet with repeat period of %d milliseconds\n\n", KEEP_ALIVE_PERIOD_ARP_MSEC ) );
break;
}
case WICED_TIMEOUT:
{
WPRINT_APP_INFO( ( "Timeout: Adding ARP packet\n\n" ) );
break;
}
default:
WPRINT_APP_INFO( ( "Error[%d]: Adding ARP packet\n\n", status ) );
break;
}
/* Get Null Function Data Frame keep alive packet info */
keep_alive_packet_info.keep_alive_id = KEEP_ALIVE_ID_NFD;
keep_alive_packet_info.packet_length = MAX_KEEP_ALIVE_PACKET_SIZE;
keep_alive_packet_info.packet = &keep_alive_packet_buffer[0];
status = wiced_wifi_get_keep_alive( &keep_alive_packet_info );
if ( status == WICED_SUCCESS )
{
print_keep_alive_info( &keep_alive_packet_info );
}
else
{
WPRINT_APP_INFO( ( "ERROR[%d]: Get keep alive packet failed for ID:%d\n", status, KEEP_ALIVE_ID_NFD) );
}
/* Get ARP keep alive packet info */
keep_alive_packet_info.keep_alive_id = KEEP_ALIVE_ID_ARP;
keep_alive_packet_info.packet_length = MAX_KEEP_ALIVE_PACKET_SIZE;
keep_alive_packet_info.packet = &keep_alive_packet_buffer[0];
status = wiced_wifi_get_keep_alive( &keep_alive_packet_info );
if ( status == WICED_SUCCESS )
{
print_keep_alive_info( &keep_alive_packet_info );
}
else
{
WPRINT_APP_INFO( ( "ERROR[%d]: Get keep alive packet failed for ID:%d\n", status, KEEP_ALIVE_ID_ARP) );
}
/* Wait 30 seconds, then disable all keep alive packets */
WPRINT_APP_INFO( ( "Sending keep alive packets " ) );
for ( i = 0; i < 30; i++ )
{
WPRINT_APP_INFO( ( "." ) );
wiced_rtos_delay_milliseconds( 1000 );
}
WPRINT_APP_INFO( ( " done\n\n" ) );
/* Disable Null Function Data Frame keep alive packet*/
status = wiced_wifi_disable_keep_alive( KEEP_ALIVE_ID_NFD );
if ( status == WICED_SUCCESS )
{
WPRINT_APP_INFO( ( "Null Function data frame keep alive packet disabled\n" ) );
}
else
{
WPRINT_APP_INFO( ( "ERROR[%d]: Failed to disable NFD keep alive packet\n", status) );
}
/* Disable ARP keep alive packet*/
status = wiced_wifi_disable_keep_alive( KEEP_ALIVE_ID_ARP );
if ( status == WICED_SUCCESS )
{
WPRINT_APP_INFO( ( "ARP keep alive packet disabled\n" ) );
}
else
{
WPRINT_APP_INFO( ( "ERROR[%d]: Failed to disable ARP keep alive packet\n", status) );
}
WPRINT_APP_INFO( ( "\r\nStop.\n") );
while ( 1 )
{
}
}
/**
* Implements a very simple DHCP server.
*
* Server will always offer next available address to a DISCOVER command
* Server will NAK any REQUEST command which is not requesting the next available address
* Server will ACK any REQUEST command which is for the next available address, and then increment the next available address
*
* @param my_addr : local IP address for binding of server port.
*/
static void dhcp_thread( uint32_t thread_input )
{
wiced_packet_t* received_packet;
wiced_packet_t* transmit_packet;
wiced_ip_address_t local_ip_address;
wiced_ip_address_t netmask;
uint32_t next_available_ip_addr;
uint32_t ip_mask;
uint32_t subnet;
uint32_t netmask_htobe;
char* option_ptr;
wiced_dhcp_server_t* server = (wiced_dhcp_server_t*)thread_input;
uint8_t subnet_mask_option_buff[] = { 1, 4, 0, 0, 0, 0 };
uint8_t server_ip_addr_option_buff[] = { 54, 4, 0, 0, 0, 0 };
uint32_t* server_ip_addr_ptr = (uint32_t*)&server_ip_addr_option_buff[2];
uint8_t wpad_option_buff[ 2 + sizeof(WPAD_SAMPLE_URL)-1 ] = { 252, sizeof(WPAD_SAMPLE_URL)-1 };
/* Save local IP address to be sent in DHCP packets */
wiced_ip_get_ipv4_address(server->interface, &local_ip_address);
*server_ip_addr_ptr = htobe32( GET_IPV4_ADDRESS( local_ip_address ) );
/* Save the current netmask to be sent in DHCP packets as the 'subnet mask option' */
wiced_ip_get_netmask(server->interface, &netmask);
netmask_htobe = htobe32( GET_IPV4_ADDRESS(netmask) );
memcpy(&subnet_mask_option_buff[2], &netmask_htobe, 4);
/* Calculate the first available IP address which will be served - based on the netmask and the local IP */
ip_mask = ~( GET_IPV4_ADDRESS( netmask ) );
subnet = GET_IPV4_ADDRESS( local_ip_address ) & GET_IPV4_ADDRESS( netmask );
next_available_ip_addr = subnet | ( ( GET_IPV4_ADDRESS( local_ip_address ) + 1 ) & ip_mask );
/* Prepare the Web proxy auto discovery URL */
memcpy(&wpad_option_buff[2], WPAD_SAMPLE_URL, sizeof(WPAD_SAMPLE_URL)-1);
ipv4_to_string( (char*)&wpad_option_buff[2 + 7], *server_ip_addr_ptr);
/* Initialise the server quit flag */
server->quit = WICED_FALSE;
/* Loop endlessly */
while ( server->quit == WICED_FALSE )
{
uint16_t data_length;
uint16_t available_data_length;
dhcp_header_t* request_header;
/* Sleep until data is received from socket. */
if ( wiced_udp_receive( &server->socket, &received_packet, WICED_WAIT_FOREVER ) != WICED_SUCCESS )
{
continue;
}
/* Get a pointer to the data in the packet */
wiced_packet_get_data( received_packet, 0, (uint8_t**) &request_header, &data_length, &available_data_length );
/* Check DHCP command */
switch ( request_header->options[2] )
{
case DHCPDISCOVER:
{
dhcp_header_t* reply_header;
uint16_t available_space;
wiced_mac_t client_mac_address;
wiced_ip_address_t client_ip_address;
uint32_t temp;
/* Create reply packet */
if ( wiced_packet_create_udp( &server->socket, sizeof(dhcp_header_t), &transmit_packet, (uint8_t**) &reply_header, &available_space ) != WICED_SUCCESS )
{
/* Cannot reply - release incoming packet */
wiced_packet_delete( received_packet );
break;
}
/* Copy in the DHCP header content from the received discover packet into the reply packet */
memcpy( reply_header, request_header, sizeof(dhcp_header_t) - sizeof(reply_header->options) );
/* Finished with the received discover packet - release it */
wiced_packet_delete( received_packet );
/* Now construct the OFFER response */
reply_header->opcode = BOOTP_OP_REPLY;
/* Clear the DHCP options list */
memset( &reply_header->options, 0, sizeof( reply_header->options ) );
/* Record client MAC address */
memcpy( &client_mac_address, request_header->client_hardware_addr, sizeof( client_mac_address ) );
/* Check whether device is already cached */
if ( get_client_ip_address_from_cache( &client_mac_address, &client_ip_address ) != WICED_SUCCESS )
{
/* Address not found in cache. Use next available IP address */
client_ip_address.version = WICED_IPV4;
client_ip_address.ip.v4 = next_available_ip_addr;
}
/* Create the IP address for the Offer */
temp = htonl(client_ip_address.ip.v4);
memcpy( reply_header->your_ip_addr, &temp, sizeof( temp ) );
/* Copy the magic DHCP number */
memcpy( reply_header->magic, dhcp_magic_cookie, 4 );
/* Add options */
option_ptr = (char *) &reply_header->options;
option_ptr = MEMCAT( option_ptr, dhcp_offer_option_buff, 3 ); /* DHCP message type */
option_ptr = MEMCAT( option_ptr, server_ip_addr_option_buff, 6 ); /* Server identifier */
option_ptr = MEMCAT( option_ptr, lease_time_option_buff, 6 ); /* Lease Time */
option_ptr = MEMCAT( option_ptr, subnet_mask_option_buff, 6 ); /* Subnet Mask */
option_ptr = (char*)MEMCAT( option_ptr, wpad_option_buff, sizeof(wpad_option_buff) ); /* Web proxy auto discovery URL */
/* Copy the local IP into the Router & DNS server Options */
memcpy( option_ptr, server_ip_addr_option_buff, 6 ); /* Router (gateway) */
option_ptr[0] = 3; /* Router id */
option_ptr += 6;
memcpy( option_ptr, server_ip_addr_option_buff, 6 ); /* DNS server */
option_ptr[0] = 6; /* DNS server id */
option_ptr += 6;
option_ptr = MEMCAT( option_ptr, mtu_option_buff, 4 ); /* Interface MTU */
option_ptr[0] = (char) 0xff; /* end options */
option_ptr++;
/* Send OFFER reply packet */
wiced_packet_set_data_end( transmit_packet, (uint8_t*) option_ptr );
if ( wiced_udp_send( &server->socket, WICED_IP_BROADCAST, IPPORT_DHCPC, transmit_packet ) != WICED_SUCCESS )
{
wiced_packet_delete( transmit_packet );
}
}
break;
case DHCPREQUEST:
{
/* REQUEST command - send back ACK or NAK */
uint32_t temp;
uint32_t* server_id_req;
dhcp_header_t* reply_header;
uint16_t available_space;
wiced_mac_t client_mac_address;
wiced_ip_address_t given_ip_address;
wiced_ip_address_t requested_ip_address;
wiced_bool_t next_avail_ip_address_used = WICED_FALSE;
/* Check that the REQUEST is for this server */
server_id_req = (uint32_t*) find_option( request_header, 54 );
if ( ( server_id_req != NULL ) && ( GET_IPV4_ADDRESS( local_ip_address ) != htobe32(*server_id_req) ) )
{
break; /* Server ID does not match local IP address */
}
/* Create reply packet */
if ( wiced_packet_create_udp( &server->socket, sizeof(dhcp_header_t), &transmit_packet, (uint8_t**) &reply_header, &available_space ) != WICED_SUCCESS )
{
/* Cannot reply - release incoming packet */
wiced_packet_delete( received_packet );
break;
}
/* Copy in the DHCP header content from the received request packet into the reply packet */
memcpy( reply_header, request_header, sizeof(dhcp_header_t) - sizeof(reply_header->options) );
/* Record client MAC address */
memcpy( &client_mac_address, request_header->client_hardware_addr, sizeof( client_mac_address ) );
/* Locate the requested address in the options and keep requested address */
requested_ip_address.version = WICED_IPV4;
requested_ip_address.ip.v4 = ntohl(*(uint32_t*)find_option( request_header, 50 ));
/* Delete received packet. We don't need it anymore */
wiced_packet_delete( received_packet );
reply_header->opcode = BOOTP_OP_REPLY;
/* Blank options list */
memset( &reply_header->options, 0, sizeof( reply_header->options ) );
/* Copy DHCP magic number into packet */
memcpy( reply_header->magic, dhcp_magic_cookie, 4 );
option_ptr = (char *) &reply_header->options;
/* Check if device is cache. If it is, give the previous IP address. Otherwise give the next available IP address */
if ( get_client_ip_address_from_cache( &client_mac_address, &given_ip_address ) != WICED_SUCCESS )
{
/* Address not found in cache. Use next available IP address */
next_avail_ip_address_used = WICED_TRUE;
given_ip_address.version = WICED_IPV4;
given_ip_address.ip.v4 = next_available_ip_addr;
}
/* Check if the requested IP address matches one we have assigned */
if ( memcmp( &requested_ip_address.ip.v4, &given_ip_address.ip.v4, sizeof( requested_ip_address.ip.v4 ) ) != 0 )
{
/* Request is not for the assigned IP - force client to take next available IP by sending NAK */
/* Add appropriate options */
option_ptr = (char*)MEMCAT( option_ptr, dhcp_nak_option_buff, 3 ); /* DHCP message type */
option_ptr = (char*)MEMCAT( option_ptr, server_ip_addr_option_buff, 6 ); /* Server identifier */
memset( reply_header->your_ip_addr, 0, sizeof( reply_header->your_ip_addr ) ); /* Clear IP addr */
}
else
{
/* Request is for next available IP */
/* Add appropriate options */
option_ptr = (char*)MEMCAT( option_ptr, dhcp_ack_option_buff, 3 ); /* DHCP message type */
option_ptr = (char*)MEMCAT( option_ptr, server_ip_addr_option_buff, 6 ); /* Server identifier */
option_ptr = (char*)MEMCAT( option_ptr, lease_time_option_buff, 6 ); /* Lease Time */
option_ptr = (char*)MEMCAT( option_ptr, subnet_mask_option_buff, 6 ); /* Subnet Mask */
option_ptr = (char*)MEMCAT( option_ptr, wpad_option_buff, sizeof(wpad_option_buff) ); /* Web proxy auto discovery URL */
/* Copy the local IP into the Router & DNS server Options */
memcpy( option_ptr, server_ip_addr_option_buff, 6 ); /* Router (gateway) */
option_ptr[0] = 3; /* Router id */
option_ptr += 6;
memcpy( option_ptr, server_ip_addr_option_buff, 6 ); /* DNS server */
option_ptr[0] = 6; /* DNS server id */
option_ptr += 6;
option_ptr = (char*)MEMCAT( option_ptr, mtu_option_buff, 4 ); /* Interface MTU */
/* Create the IP address for the Offer */
temp = htonl(given_ip_address.ip.v4);
memcpy( reply_header->your_ip_addr, &temp, sizeof( temp ) );
/* Increment next available IP address only if not found in cache */
if ( next_avail_ip_address_used == WICED_TRUE )
{
do
{
next_available_ip_addr = subnet | ( ( next_available_ip_addr + 1 ) & ip_mask );
} while ( next_available_ip_addr == GET_IPV4_ADDRESS(local_ip_address) );
}
/* Cache client */
add_client_to_cache( &client_mac_address, &given_ip_address );
}
option_ptr[0] = (char) 0xff; /* end options */
option_ptr++;
/* Send reply packet */
wiced_packet_set_data_end( transmit_packet, (uint8_t*) option_ptr );
if ( wiced_udp_send( &server->socket, WICED_IP_BROADCAST, IPPORT_DHCPC, transmit_packet ) != WICED_SUCCESS )
{
wiced_packet_delete( transmit_packet );
}
}
break;
default:
/* Unknown packet type - release received packet */
wiced_packet_delete( received_packet );
break;
}
}
/* Server loop has exited due to quit flag */
/* Delete DHCP socket */
wiced_udp_delete_socket( &server->socket );
WICED_END_OF_CURRENT_THREAD( );
}