vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Com/elinux/vp_com.c

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