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

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