C_RESULT vp_com_serial_disconnect(vp_com_serial_config_t* config, vp_com_connection_t* connection)
{
C_RESULT res = (connection == NULL) ? VP_COM_PARAMERROR : VP_COM_OK;
VP_COM_CHECK( res );
connection->is_up = 0;
return VP_COM_OK;
}
C_RESULT vp_com_serial_connect(vp_com_t* vp_com, vp_com_connection_t* connection, int32_t numAttempts)
{
C_RESULT res = VP_COM_OK;
res = (connection == NULL) ? VP_COM_PARAMERROR : VP_COM_OK;
VP_COM_CHECK( res );
connection->is_up = 1;
return VP_COM_OK;
}
C_RESULT vp_com_wf_connect(vp_com_t* vp_com, vp_com_wifi_connection_t* connection, int32_t numAttempts)
{
C_RESULT res = VP_COM_OK;
#ifdef USE_IWLIB
int32_t wlsock;
vp_com_wifi_config_t* config = (vp_com_wifi_config_t*)vp_com->config;
wireless_config iwconf;
wlsock = iw_sockets_open();
res = ( wlsock < 0 ) ? VP_COM_ERROR : VP_COM_OK;
VP_COM_CHECK( res );
iw_get_basic_config( wlsock, config->itfName, &iwconf );
iwconf.has_nwid = 0;
iwconf.has_freq = 0;
iwconf.has_key = 0;
iwconf.has_mode = 1;
iwconf.mode = config->infrastructure ? IW_MODE_INFRA : IW_MODE_ADHOC;
iwconf.has_essid = 1;
iwconf.essid_on = 1;
strncpy( &iwconf.essid[0], connection->networkName, IW_ESSID_MAX_SIZE+1 );
res = iw_set_basic_config( wlsock, config->itfName, &iwconf ) < 0 ? C_FAIL : C_OK;
if( SUCCEED(res) )
PRINT(" OK!\n");
else
PRINT(" FAILED!\n");
iw_sockets_close(wlsock);
#endif
return res;
}
C_RESULT vp_com_open_socket(vp_com_socket_t* sck, Read* read, Write* write)
{
C_RESULT res = VP_COM_OK;
BOOL reuseaddroption = TRUE;
BOOL exclusiveaddroption = FALSE;
SOCKET s = -1;
struct sockaddr_in name = { 0 };
struct sockaddr_in local_address = { 0 };
struct sockaddr_in remote_address = { 0 };
int err;
int res_setsockopt=0,res_connect=0,res_bind=0;
switch( sck->protocol )
{
case VP_COM_TCP:
s = socket( AF_INET, SOCK_STREAM, IPPROTO_TCP );
res = ( s == INVALID_SOCKET ) ? VP_COM_ERROR : VP_COM_OK;
break;
case VP_COM_UDP:
s = socket( AF_INET, SOCK_DGRAM, IPPROTO_UDP );
sck->scn = inet_addr(sck->serverHost); // Cache destination in int format
res = ( s == INVALID_SOCKET ) ? VP_COM_ERROR : VP_COM_OK;
break;
default:
sck->type = VP_COM_CLIENT;
res = VP_COM_PARAMERROR;
break;
}
if( VP_FAILED(res) )
{
PRINT("\nSocket opening failed\n");
}
VP_COM_CHECK( res );
// res_setsockopt = setsockopt(s,SOL_SOCKET,SO_REUSEADDR,(char*)&reuseaddroption,sizeof(reuseaddroption));
res_setsockopt = setsockopt(s,SOL_SOCKET,SO_EXCLUSIVEADDRUSE,(char*)&exclusiveaddroption,sizeof(exclusiveaddroption));
name.sin_family = AF_INET;
name.sin_port = htons( sck->port );
switch( sck->type )
{
case VP_COM_CLIENT:
remote_address.sin_family = AF_INET;
remote_address.sin_port = htons( sck->port );
remote_address.sin_addr.s_addr = inet_addr(sck->serverHost);
if ( sck->protocol ==VP_COM_UDP)
{
local_address.sin_addr.s_addr= INADDR_ANY;
local_address.sin_family = AF_INET;
local_address.sin_port = htons( sck->port ); /* Bind to any available port */
res_bind = bind(s,(const struct sockaddr*)&local_address,sizeof(local_address));
err = WSAGetLastError();
res = (res_bind==0)? VP_COM_OK : VP_COM_ERROR; /* Convert from Win32 error code to VP SDK error code */
}
if (VP_SUCCEEDED(res))// && (sck->protocol !=VP_COM_UDP))
{
res_connect = connect( s, (struct sockaddr*)&remote_address, sizeof( remote_address ) );
if( res_connect == -1 ){ res = VP_COM_ERROR; err = WSAGetLastError(); }
}
break;
case VP_COM_SERVER:
/* Local TCP/UDP address on which we wait for connections */
local_address.sin_family = AF_INET;
local_address.sin_port = htons( sck->port );
local_address.sin_addr.s_addr = INADDR_ANY; /* Accept connections on any network interface */
res_bind = bind( s, (const struct sockaddr*)&local_address, sizeof(local_address) );
res = (res_bind==0)? VP_COM_OK : VP_COM_ERROR ; /* Convert from Win32 error code to VP SDK error code */
break;
default:
res = VP_COM_PARAMERROR;
break;
}
if(res == VP_COM_OK)
{
sck->priv = (void*) s;
switch( sck->protocol )
{
case VP_COM_TCP:
if(read) *read = (Read) vp_com_read_socket;
if(write) *write = (Write) vp_com_write_socket;
break;
case VP_COM_UDP:
if(read) *read = (Read) vp_com_read_udp_socket;
if(write) *write = (Write) vp_com_write_udp_socket;
break;
default:
if(read) *read = NULL;
if(write) *write = NULL;
break;
}
}
else
{
closesocket( s );
}
if (sck->block != VP_COM_DEFAULT &&
sck->block != VP_COM_WAITALL &&
sck->block != VP_COM_DONTWAIT)
{
sck->block = VP_COM_DEFAULT;
}
return res;
}
C_RESULT vp_com_open_socket(vp_com_socket_t* sck, Read* read, Write* write)
{
C_RESULT res = VP_COM_OK;
int s = -1, type;
struct sockaddr_in name = { 0 };
switch( sck->protocol )
{
case VP_COM_TCP:
s = socket( AF_INET, SOCK_STREAM, 0 );
res = ( s < 0 ) ? VP_COM_ERROR : VP_COM_OK;
type = sck->type;
break;
case VP_COM_UDP:
s = socket( AF_INET, SOCK_DGRAM, 0 );
sck->scn = inet_addr(sck->serverHost); // Cache destination in int format
res = ( s < 0 ) ? VP_COM_ERROR : VP_COM_OK;
type = VP_COM_SERVER; // Make sure we will bind the socket in the connection-less protocol
break;
default:
type = VP_COM_CLIENT;
res = VP_COM_PARAMERROR;
break;
}
if( FAILED(res) )
{
PRINT("\nSocket opening failed\n");
}
VP_COM_CHECK( res );
name.sin_family = AF_INET;
name.sin_port = htons( sck->port );
switch( type )
{
case VP_COM_CLIENT:
name.sin_addr.s_addr = inet_addr(sck->serverHost);
if ( connect( s, (struct sockaddr*)&name, sizeof( name ) ) == -1 )
res = VP_COM_ERROR;
break;
case VP_COM_SERVER:
name.sin_addr.s_addr = INADDR_ANY;
if ( bind( s, (struct sockaddr*)&name, sizeof(struct sockaddr)) < 0 )
res = VP_COM_ERROR;
if ( sck->is_multicast == 1 )
{
if ( sck->multicast_base_addr == 0 )
{
PRINT("Error : multicast base address is not defined\n");
res = VP_COM_ERROR;
break;
}
in_addr_t multicast_address, drone_address;
vp_com_wifi_config_t *wifi_cfg = (vp_com_wifi_config_t*) wifi_config();
// compute remote address according to local address
drone_address = inet_addr(wifi_cfg->server);
multicast_address = htonl( sck->multicast_base_addr | (ntohl(drone_address) & 0xFF) );
struct ip_mreq mreq;
mreq.imr_interface.s_addr = inet_addr(wifi_cfg->localHost);
mreq.imr_multiaddr.s_addr = multicast_address;
if ( setsockopt( s, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq) ) < 0) {
PRINT("!!! Error enabling multicast !!!\n");
}
}
break;
default:
res = VP_COM_PARAMERROR;
break;
}
if(res == VP_COM_OK)
{
sck->priv = (void*) s;
switch( sck->protocol )
{
case VP_COM_TCP:
if(read) *read = (Read) vp_com_read_socket;
if(write) *write = (Write) vp_com_write_socket;
break;
case VP_COM_UDP:
if(read) *read = (Read) vp_com_read_udp_socket;
if(write) *write = (Write) vp_com_write_udp_socket;
break;
default:
if(read) *read = NULL;
if(write) *write = NULL;
break;
}
}
else
{
close( s );
}
if (sck->block != VP_COM_DEFAULT &&
sck->block != VP_COM_WAITALL &&
sck->block != VP_COM_DONTWAIT)
{
sck->block = VP_COM_DEFAULT;
}
return res;
}
