git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Com/linux/vp_com.c

   1 #include <VP_Com/vp_com.h>
   2 #include <VP_Com/vp_com_socket.h>
   3 #include <VP_Com/vp_com_error.h>
   4 #include <VP_Api/vp_api_error.h>
   5 #include <VP_Os/vp_os_malloc.h>
   6 #include <VP_Os/vp_os_signal.h>
   7 #include <VP_Os/vp_os_assert.h>
   8 #include <VP_Os/vp_os_print.h>
   9
  10 #ifdef USE_WIFI
  11 #   include "vp_com_wifi.h"
  12 #endif
  13 #ifdef USE_SERIAL
  14 #   include "vp_com_serial.h"
  15 #endif
  16 #ifdef USE_WIRED
  17 #   include "vp_com_wired.h"
  18 #endif
  19
  20 typedef C_RESULT (*VP_COM_x_init)(void);
  21 typedef C_RESULT (*VP_COM_x_shutdown)(void);
  22 typedef C_RESULT (*VP_COM_x_network_adapter_lookup)(vp_com_network_adapter_lookup_t callback);
  23 typedef C_RESULT (*VP_COM_x_inquire)(const char* deviceName, vp_com_inquiry_t callback, uint32_t timeout);
  24 typedef C_RESULT (*VP_COM_x_local_config)(vp_com_config_t* config);
  25 typedef C_RESULT (*VP_COM_x_connect)(vp_com_t* vp_com, vp_com_connection_t* connection, int32_t numAttempts);
  26 typedef C_RESULT (*VP_COM_x_disconnect)(vp_com_config_t* config, vp_com_connection_t* connection);
  27 typedef C_RESULT (*VP_COM_x_get_rssi)(vp_com_config_t* cfg, int32_t* rssi);
  28 typedef C_RESULT (*VP_COM_x_wait_connections)(vp_com_connection_t** c, vp_com_socket_t* server, vp_com_socket_t* client, int queueLength);
  29 typedef C_RESULT (*VP_COM_x_open)(vp_com_config_t* config, vp_com_connection_t* connection, vp_com_socket_t* socket, Read* read, Write* write);
  30 typedef C_RESULT (*VP_COM_x_close)(vp_com_socket_t* socket);
  31
  32 #define VP_COM_INIT                  vp_com->init
  33 #define VP_COM_SHUTDOWN              vp_com->shutdown
  34 #define VP_COM_NETWORKADAPTERLOOKUP  vp_com->network_adapter_lookup
  35 #define VP_COM_INQUIRE               vp_com->inquire
  36 #define VP_COM_LOCAL_CONFIG          vp_com->local_config
  37 #define VP_COM_CONNECT               vp_com->connect
  38 #define VP_COM_DISCONNECT            vp_com->disconnect
  39 #define VP_COM_GET_RSSI              vp_com->get_rssi
  40 #define VP_COM_WAITCONNECTIONS       vp_com->wait_connections
  41 #define VP_COM_OPEN                  vp_com->open
  42 #define VP_COM_CLOSE                 vp_com->close
  43
  44 char vp_com_pin_code[VP_COM_NAME_MAXSIZE];
  45
  46 C_RESULT vp_com_init(vp_com_t* vp_com)
  47 {
  48   C_RESULT res = VP_COM_NOTSUPPORTED;
  49
  50   VP_OS_ASSERT( vp_com != NULL );
  51
  52   if(!vp_com->initialized)
  53   {
  54     vp_os_mutex_init(&vp_com->mutex);
  55     vp_com->initialized ++;
  56   }
  57   vp_os_mutex_lock(&vp_com->mutex);
  58
  59   if(vp_com->ref_count > 0)
  60   {
  61     vp_com->ref_count ++;
  62     res = VP_COM_OK;
  63   }
  64   else
  65   {
  66 #ifndef NO_COM
  67 #ifdef USE_BLUEZ
  68     if(vp_com->type == VP_COM_BLUETOOTH)
  69     {
  70       vp_com->init                    = (VP_COM_x_init) vp_com_bt_init;
  71       vp_com->shutdown                = (VP_COM_x_shutdown) vp_com_bt_shutdown;
  72       vp_com->network_adapter_lookup  = (VP_COM_x_network_adapter_lookup) vp_com_bt_network_adapter_lookup;
  73       vp_com->local_config            = (VP_COM_x_local_config) vp_com_bt_local_config;
  74       vp_com->inquire                 = (VP_COM_x_inquire) vp_com_bt_inquire;
  75       vp_com->connect                 = (VP_COM_x_connect) vp_com_bt_connect;
  76       vp_com->disconnect              = (VP_COM_x_disconnect) vp_com_bt_disconnect;
  77       vp_com->get_rssi                = NULL;
  78       vp_com->wait_connections        = (VP_COM_x_wait_connections) vp_com_bt_wait_connections;
  79       vp_com->open                    = (VP_COM_x_open) vp_com_bt_open;
  80       vp_com->close                   = (VP_COM_x_close) vp_com_bt_close;
  81     }
  82 #endif
  83
  84 #ifdef USE_WIFI
  85     if(vp_com->type == VP_COM_WIFI)
  86     {
  87       vp_com->init                    = (VP_COM_x_init) vp_com_wf_init;
  88       vp_com->shutdown                = (VP_COM_x_shutdown) vp_com_wf_shutdown;
  89       vp_com->network_adapter_lookup  = (VP_COM_x_network_adapter_lookup) vp_com_wf_network_adapter_lookup;
  90       vp_com->local_config            = (VP_COM_x_local_config) vp_com_wf_local_config;
  91       vp_com->inquire                 = (VP_COM_x_inquire) vp_com_wf_inquire;
  92       vp_com->connect                 = (VP_COM_x_connect) vp_com_wf_connect;
  93       vp_com->disconnect              = (VP_COM_x_disconnect) vp_com_wf_disconnect;
  94       vp_com->get_rssi                = (VP_COM_x_get_rssi) vp_com_wf_get_rssi;
  95       vp_com->wait_connections        = (VP_COM_x_wait_connections) vp_com_wf_wait_connections;
  96       vp_com->open                    = (VP_COM_x_open) vp_com_wf_open;
  97       vp_com->close                   = (VP_COM_x_close) vp_com_wf_close;
  98     }
  99 #endif // > USE_WIFI
 100 #endif // > NO_COM
 101
 102 #ifdef USE_SERIAL
 103     if(vp_com->type == VP_COM_SERIAL)
 104     {
 105       vp_com->init                    = (VP_COM_x_init) vp_com_serial_init;
 106       vp_com->shutdown                = (VP_COM_x_shutdown) vp_com_serial_shutdown;
 107       vp_com->network_adapter_lookup  = (VP_COM_x_network_adapter_lookup) vp_com_serial_network_adapter_lookup;
 108       vp_com->local_config            = (VP_COM_x_local_config) vp_com_serial_local_config;
 109       vp_com->inquire                 = (VP_COM_x_inquire) vp_com_serial_inquire;
 110       vp_com->connect                 = (VP_COM_x_connect) vp_com_serial_connect;
 111       vp_com->disconnect              = (VP_COM_x_disconnect) vp_com_serial_disconnect;
 112       vp_com->get_rssi                = NULL;
 113       vp_com->wait_connections        = (VP_COM_x_wait_connections) vp_com_serial_wait_connections;
 114       vp_com->open                    = (VP_COM_x_open) vp_com_serial_open;
 115       vp_com->close                   = (VP_COM_x_close) vp_com_serial_close;
 116     }
 117 #endif
 118
 119 #ifdef USE_WIRED
 120     if( vp_com->type == VP_COM_WIRED )
 121     {
 122       vp_com->init                    = (VP_COM_x_init) vp_com_wired_init;
 123       vp_com->shutdown                = (VP_COM_x_shutdown) vp_com_wired_shutdown;
 124       vp_com->network_adapter_lookup  = (VP_COM_x_network_adapter_lookup) vp_com_wired_network_adapter_lookup;
 125       vp_com->local_config            = (VP_COM_x_local_config) vp_com_wired_local_config;
 126       vp_com->inquire                 = (VP_COM_x_inquire) vp_com_wired_inquire;
 127       vp_com->connect                 = (VP_COM_x_connect) vp_com_wired_connect;
 128       vp_com->disconnect              = (VP_COM_x_disconnect) vp_com_wired_disconnect;
 129       vp_com->get_rssi                = NULL;
 130       vp_com->wait_connections        = (VP_COM_x_wait_connections) vp_com_wired_wait_connections;
 131       vp_com->open                    = (VP_COM_x_open) vp_com_wired_open;
 132       vp_com->close                   = (VP_COM_x_close) vp_com_wired_close;
 133     }
 134 #endif
 135     if(VP_COM_INIT)
 136       res = VP_COM_INIT();
 137
 138     if(res == VP_COM_OK)
 139     {
 140       vp_os_install_error_handler( VP_COM_SDK_SIGNATURE, vp_com_formatMessage );
 141       vp_com->ref_count ++;
 142     }
 143   }
 144
 145   vp_os_mutex_unlock( &vp_com->mutex );
 146
 147   return res;
 148 }
 149
 150 C_RESULT vp_com_shutdown(vp_com_t* vp_com)
 151 {
 152   VP_OS_ASSERT( vp_com != NULL );
 153
 154   vp_os_mutex_lock( &vp_com->mutex );
 155
 156   if(vp_com->ref_count > 0)
 157   {
 158     vp_com->ref_count--;
 159     if(vp_com->ref_count == 0)
 160     {
 161       vp_os_mutex_unlock( &vp_com->mutex );
 162       vp_os_mutex_destroy( &vp_com->mutex );
 163
 164       return VP_COM_SHUTDOWN();
 165     }
 166   }
 167
 168   vp_os_mutex_unlock(&vp_com->mutex);
 169
 170   return VP_COM_OK;
 171 }
 172 #ifdef USE_BLUEZ
 173 C_RESULT vp_com_str_to_address(const char* address, bdaddr_t* addr)
 174 {
 175   str2ba( address,addr );
 176
 177   return VP_COM_OK;
 178 }
 179
 180 C_RESULT vp_com_address_to_str(const bdaddr_t* addr, char* address)
 181 {
 182   ba2str( addr, address );
 183
 184   return VP_COM_OK;
 185 }
 186 #endif
 187
 188 C_RESULT vp_com_copy_address(const bdaddr_t* from,bdaddr_t* to)
 189 {
 190   vp_os_memcpy( to, from, sizeof( bdaddr_t ) );
 191
 192   return VP_COM_OK;
 193 }
 194
 195 C_RESULT vp_com_cmp_address(const bdaddr_t* bd1, const bdaddr_t* bd2)
 196 {
 197   int32_t i;
 198
 199   for( i = 0; i < BDADDR_SIZE && ( bd1->b[i] == bd2->b[i] ); i++ );
 200
 201   return ( i < BDADDR_SIZE ) ? VP_COM_ERROR : VP_COM_OK;
 202 }
 203
 204 C_RESULT vp_com_network_adapter_lookup(vp_com_t* vp_com, vp_com_network_adapter_lookup_t callback)
 205 {
 206   return VP_COM_NETWORKADAPTERLOOKUP( callback );
 207 }
 208
 209 C_RESULT vp_com_local_config(vp_com_t* vp_com, vp_com_config_t* config)
 210 {
 211   C_RESULT res = C_OK;
 212
 213   VP_OS_ASSERT( vp_com != NULL );
 214
 215   if( vp_com->config != config )
 216   {
 217     res = VP_COM_LOCAL_CONFIG(config);
 218
 219     if( SUCCEED( res ) )
 220       vp_com->config = config;
 221
 222   }
 223
 224   return res;
 225 }
 226
 227 C_RESULT vp_com_inquire(vp_com_t* vp_com, const char* deviceName, vp_com_inquiry_t callback, uint32_t timeout)
 228 {
 229   VP_OS_ASSERT( vp_com != NULL );
 230
 231   return VP_COM_INQUIRE( deviceName, callback, timeout );
 232 }
 233
 234 C_RESULT vp_com_connect(vp_com_t* vp_com, vp_com_connection_t* connection, uint32_t numAttempts)
 235 {
 236   C_RESULT res = VP_COM_OK;
 237   bool_t already_connected;
 238
 239   VP_OS_ASSERT( vp_com != NULL );
 240
 241   if(vp_com->config != NULL)
 242   {
 243     vp_os_mutex_lock(&vp_com->mutex);
 244
 245     already_connected = vp_com->connection && vp_com->connection->is_up == 1;
 246
 247     // TODO voir pour ajouter un test sur l'adresse ethernet de la connection
 248     if( already_connected && vp_com->connection != connection )
 249     {
 250       already_connected = FALSE;
 251       vp_com_disconnect(vp_com);
 252     }
 253
 254     if( !already_connected )
 255     {
 256       res = VP_COM_CONNECT(vp_com, connection, numAttempts);
 257
 258       if( SUCCEED( res ) )
 259       {
 260         vp_com->connection = connection;
 261         vp_com->connection->is_up = 1;
 262       }
 263     }
 264
 265     vp_os_mutex_unlock(&vp_com->mutex);
 266   }
 267
 268   return res;
 269 }
 270
 271 C_RESULT vp_com_disconnect(vp_com_t* vp_com)
 272 {
 273   C_RESULT res = VP_COM_ERROR;
 274
 275   VP_OS_ASSERT( vp_com != NULL );
 276
 277   if(vp_com->config != NULL && vp_com->connection != NULL)
 278   {
 279     vp_os_mutex_lock(&vp_com->mutex);
 280
 281     res = VP_COM_DISCONNECT(vp_com->config, vp_com->connection);
 282
 283     if( SUCCEED( res ) )
 284       vp_com->connection->is_up = 0;
 285
 286
 287     vp_os_mutex_unlock(&vp_com->mutex);
 288   }
 289
 290   return res;
 291 }
 292
 293 C_RESULT vp_com_get_rssi(vp_com_t* vp_com, int32_t* rssi)
 294 {
 295   C_RESULT res;
 296
 297   if( vp_com != NULL && vp_com->config != NULL && vp_com->get_rssi != NULL )
 298     res = VP_COM_GET_RSSI( vp_com->config, rssi );
 299   else
 300   {
 301     *rssi = 0;
 302     res = C_FAIL;
 303   }
 304
 305   return res;
 306 }
 307
 308 C_RESULT vp_com_wait_connections(vp_com_t* vp_com, vp_com_socket_t* server, vp_com_socket_t* client, int32_t queueLength)
 309 {
 310   VP_OS_ASSERT( vp_com != NULL );
 311
 312   return VP_COM_WAITCONNECTIONS( &vp_com->connection, server, client, queueLength );
 313 }
 314
 315 C_RESULT vp_com_open(vp_com_t* vp_com, vp_com_socket_t* socket,Read* read,Write* write)
 316 {
 317   VP_OS_ASSERT( vp_com != NULL );
 318
 319   if( vp_com->ref_count > 0 )
 320     return VP_COM_OPEN(vp_com->config, vp_com->connection, socket, read, write);
 321
 322   PRINT("[VP_COM] Trying to open a socket with a non initialized vp_com object\n");
 323
 324   return C_FAIL;
 325 }
 326
 327 C_RESULT vp_com_close(vp_com_t* vp_com, vp_com_socket_t* socket)
 328 {
 329   VP_OS_ASSERT( vp_com != NULL );
 330
 331   if( vp_com->ref_count > 0 )
 332     return VP_COM_CLOSE( socket );
 333
 334   PRINT("[VP_COM] Trying to close a socket with a non initialized vp_com object\n");
 335
 336   return C_FAIL;
 337 }
 338
 339 C_RESULT vp_com_sockopt(vp_com_t* vp_com, vp_com_socket_t* socket, VP_COM_SOCKET_OPTIONS options)
 340 {
 341   C_RESULT res;
 342
 343   switch( socket->protocol )
 344   {
 345 #ifndef NO_COM
 346     case VP_COM_TCP:
 347       res = vp_com_sockopt_ip(vp_com, socket, options);
 348       break;
 349 #endif
 350     case VP_COM_SERIAL:
 351       res = VP_COM_OK;
 352       break;
 353
 354     default:
 355       res = VP_COM_ERROR;
 356       break;
 357   }
 358
 359   return res;
 360 }