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

   1 #include <VP_Com/vp_com_socket.h>
   2 #include <VP_Com/vp_com_error.h>
   3
   4 #include <VP_Os/vp_os_malloc.h>
   5 #include <VP_Os/vp_os_print.h>
   6 #include <VP_Os/vp_os_signal.h>
   7
   8 #include <fcntl.h>
   9 #include <errno.h>
  10
  11 #ifdef __linux__
  12 #include <sys/socket.h>
  13 #include <sys/ioctl.h>
  14 #include <netinet/in.h>
  15 #include <netinet/tcp.h>
  16 #include <unistd.h>
  17
  18 #define CYGPKG_NET 1
  19 #endif
  20
  21 #if defined(USE_MINGW32) || defined(TARGET_OS_IPHONE) || defined(TARGET_IPHONE_SIMULATOR)
  22
  23 #define MSG_NOSIGNAL 0
  24
  25 #define CYGPKG_NET 1
  26 #endif // USE_MINGW32 || TARGET_OS_IPHONE || TARGET_IPHONE_SIMULATOR
  27
  28 #ifdef CYGPKG_NET
  29
  30 #ifndef USE_MINGW32
  31 extern int inet_addr(const char *);
  32 #endif
  33
  34 C_RESULT vp_com_open_socket(vp_com_socket_t* sck, Read* read, Write* write)
  35 {
  36   C_RESULT res = VP_COM_OK;
  37
  38   int s = -1, type;
  39   struct sockaddr_in name = { 0 };
  40
  41   switch( sck->protocol )
  42   {
  43     case VP_COM_TCP:
  44       s = socket( AF_INET, SOCK_STREAM, 0 );
  45       res = ( s < 0 ) ? VP_COM_ERROR : VP_COM_OK;
  46       type = sck->type;
  47       break;
  48
  49     case VP_COM_UDP:
  50       s = socket( AF_INET, SOCK_DGRAM, 0 );
  51       sck->scn  = inet_addr(sck->serverHost); // Cache destination in int format
  52       res = ( s < 0 ) ? VP_COM_ERROR : VP_COM_OK;
  53       type = VP_COM_SERVER; // Make sure we will bind the socket in the connection-less protocol
  54       break;
  55
  56     default:
  57       type = VP_COM_CLIENT;
  58       res = VP_COM_PARAMERROR;
  59       break;
  60   }
  61
  62   if( FAILED(res) )
  63   {
  64     PRINT("\nSocket opening failed\n");
  65   }
  66
  67   VP_COM_CHECK( res );
  68
  69   name.sin_family = AF_INET;
  70   name.sin_port   = htons( sck->port );
  71   switch( type )
  72   {
  73     case VP_COM_CLIENT:
  74       name.sin_addr.s_addr  = inet_addr(sck->serverHost);
  75       if ( connect( s, (struct sockaddr*)&name, sizeof( name ) ) == -1 )
  76         res = VP_COM_ERROR;
  77       break;
  78
  79     case VP_COM_SERVER:
  80       name.sin_addr.s_addr  = INADDR_ANY;
  81
  82       if ( bind( s, (struct sockaddr*)&name, sizeof(struct sockaddr)) < 0 )
  83         res = VP_COM_ERROR;
  84
  85       if ( sck->is_multicast == 1 )
  86       {
  87
  88         if ( sck->multicast_base_addr == 0 )
  89         {
  90           PRINT("Error : multicast base address is not defined\n");
  91           res = VP_COM_ERROR;
  92           break;
  93         }
  94
  95         in_addr_t multicast_address, drone_address;
  96         vp_com_wifi_config_t *wifi_cfg = (vp_com_wifi_config_t*) wifi_config();
  97
  98         // compute remote address according to local address
  99         drone_address = inet_addr(wifi_cfg->server);
 100         multicast_address = htonl( sck->multicast_base_addr | (ntohl(drone_address) & 0xFF) );
 101
 102         struct ip_mreq mreq;
 103         mreq.imr_interface.s_addr = inet_addr(wifi_cfg->localHost);
 104         mreq.imr_multiaddr.s_addr = multicast_address;
 105
 106         if ( setsockopt( s, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq) ) < 0) {
 107           PRINT("!!! Error enabling multicast !!!\n");
 108         }
 109       }
 110
 111       break;
 112
 113     default:
 114       res = VP_COM_PARAMERROR;
 115       break;
 116   }
 117
 118   if(res == VP_COM_OK)
 119   {
 120     sck->priv = (void*) s;
 121
 122     switch( sck->protocol )
 123     {
 124       case VP_COM_TCP:
 125         if(read)  *read   = (Read) vp_com_read_socket;
 126         if(write) *write  = (Write) vp_com_write_socket;
 127         break;
 128
 129       case VP_COM_UDP:
 130         if(read)  *read   = (Read) vp_com_read_udp_socket;
 131         if(write) *write  = (Write) vp_com_write_udp_socket;
 132         break;
 133
 134       default:
 135         if(read)  *read   = NULL;
 136         if(write) *write  = NULL;
 137         break;
 138     }
 139   }
 140   else
 141   {
 142     close( s );
 143   }
 144
 145   if (sck->block != VP_COM_DEFAULT &&
 146       sck->block != VP_COM_WAITALL &&
 147       sck->block != VP_COM_DONTWAIT)
 148   {
 149     sck->block = VP_COM_DEFAULT;
 150   }
 151
 152   return res;
 153 }
 154
 155 C_RESULT vp_com_close_socket(vp_com_socket_t* socket)
 156 {
 157   if(socket == NULL)
 158     return VP_COM_PARAMERROR;
 159
 160   // shutdown( (int) socket->priv, SHUT_RDWR );
 161   close( (int) socket->priv );
 162
 163   socket->priv = NULL;
 164
 165   return VP_COM_OK;
 166 }
 167
 168 C_RESULT vp_com_wait_socket(vp_com_socket_t* server, vp_com_socket_t* client, int32_t queue_length)
 169 {
 170   C_RESULT res = VP_COM_OK;
 171
 172   if(server == NULL)
 173     return VP_COM_PARAMERROR;
 174
 175   int s = (int) server->priv;
 176   int c = 0;
 177   socklen_t l = sizeof(struct sockaddr_in);
 178   struct sockaddr_in raddr = { 0 }; // remote address
 179
 180   server->queue_length = queue_length;
 181
 182   listen(s, queue_length);
 183   c = accept( s, (struct sockaddr*)&raddr, &l );
 184   if( c < 0 )
 185     res = VP_COM_ERROR;
 186
 187   if(SUCCEED( res ))
 188   {
 189     vp_os_memcpy( client, server, sizeof(vp_com_socket_t) );
 190     client->priv = (void*) c;
 191   }
 192
 193   return res;
 194 }
 195
 196 #endif
 197
 198 C_RESULT vp_com_sockopt_ip(vp_com_t* vp_com, vp_com_socket_t* socket, VP_COM_SOCKET_OPTIONS options)
 199 {
 200   int32_t one  = 1;
 201   int32_t zero = 0;
 202
 203   C_RESULT res = VP_COM_ERROR;
 204 #ifdef CYGPKG_NET
 205   int s = (int) socket->priv;
 206
 207   if( options & VP_COM_NON_BLOCKING )
 208   {
 209 #ifndef USE_MINGW32
 210     int32_t arg = 1;
 211
 212     PRINT("Setting socket %d to non blocking\n", s);
 213     res = ioctl( s, FIONBIO, &arg ) < 0 ? C_FAIL : C_OK;
 214 #endif
 215
 216     if( FAILED(res) )
 217       PRINT("error setting non blocking\n");
 218   }
 219
 220   if( options & VP_COM_NO_DELAY )
 221   {
 222     PRINT("Disabling the Nagle (TCP No Delay) algorithm for socket %d\n", s);
 223
 224     res = setsockopt( s, IPPROTO_TCP, TCP_NODELAY, (char *)&one, sizeof(one) ) < 0 ? C_FAIL : C_OK;
 225
 226     if( FAILED(res) )
 227       PRINT("error disabling the Nagle algorithm\n");
 228   }
 229
 230   if ( options & VP_COM_MULTICAST_ON )
 231   {
 232     res = setsockopt( s, IPPROTO_IP, IP_MULTICAST_LOOP, &zero, sizeof(zero) );
 233     if ( FAILED(res) )
 234       PRINT("Error : cannot set IP_MULTICAST_LOOP to 0\n");
 235
 236     struct in_addr interface_addr;
 237     interface_addr.s_addr = INADDR_ANY;
 238     res = setsockopt( s, IPPROTO_IP, IP_MULTICAST_IF, &interface_addr, sizeof(interface_addr) );
 239     if ( FAILED(res) ) {
 240       PRINT("Error : cannot enable multicast.\n");
 241     }
 242   }
 243 #endif
 244   return res;
 245 }
 246
 247 /////////////////////////////////////////////////////////////////////////////////////////////////////////////
 248
 249 #ifdef CYGPKG_NET
 250
 251 C_RESULT vp_com_read_udp_socket(vp_com_socket_t* sck, int8_t* buffer, int32_t* size)
 252 {
 253   C_RESULT res;
 254   int s = (int) sck->priv;
 255   struct sockaddr_in from;
 256
 257   socklen_t from_len = sizeof(from);
 258
 259   int flags = MSG_NOSIGNAL;
 260   if (VP_COM_DONTWAIT == sck->block)
 261     flags |= MSG_DONTWAIT;
 262   else if (VP_COM_WAITALL == sck->block)
 263     flags |= MSG_WAITALL;
 264
 265
 266   if(s >= 0)
 267   {
 268     res = VP_COM_OK;
 269     *size = recvfrom(s, (char*)buffer, *size, flags, (struct sockaddr*)&from, &from_len );
 270
 271     if(*size < 0)
 272     {
 273
 274 #ifdef USE_MINGW32
 275       switch( WSAGetLastError() )
 276       {
 277         case WSAEOPNOTSUPP:
 278           PRINT("MSG_NOSIGNAL is not supported on this platform\n");
 279           res = VP_COM_ERROR;
 280           break;
 281
 282         case WSAEINTR:
 283           *size = 0;
 284           break;
 285
 286         case WSAENETDOWN:
 287         case WSAETIMEDOUT:
 288         case WSAECONNRESET:
 289           PRINT("Connection with peer is not enabled\n");
 290           res = VP_COM_ERROR;
 291           break;
 292       }
 293 #else
 294       switch( errno )
 295       {
 296         case EAGAIN:
 297         case EINTR:
 298           *size = 0;
 299           break;
 300
 301         case EOPNOTSUPP:
 302           PRINT("MSG_NOSIGNAL is not supported on this platform\n");
 303           res = VP_COM_ERROR;
 304           break;
 305
 306         case EPIPE:
 307         case ENOTCONN:
 308         case ECONNRESET:
 309           PRINT("Connection with peer is not enabled\n");
 310           res = VP_COM_ERROR;
 311           break;
 312
 313         default:
 314           PRINT("recvfrom fails with error: %s\n", strerror(errno));
 315           res = VP_COM_ERROR;
 316           break;
 317       }
 318 #endif // USE_MINGW32
 319     }
 320     else
 321     {
 322       sck->scn   = from.sin_addr.s_addr;
 323       sck->port  = ntohs(from.sin_port);
 324     }
 325   }
 326   else
 327   {
 328     res = VP_COM_ERROR;
 329   }
 330
 331   return res;
 332 }
 333
 334 C_RESULT vp_com_make_udp_target( vp_com_socket_t* sck )
 335 {
 336   C_RESULT res = C_FAIL;
 337
 338   if( sck->protocol == VP_COM_UDP )
 339   {
 340     sck->scn  = inet_addr(sck->serverHost); // We use scn field to store ip in order to avoid a call to inet_addr each time we call write
 341     res = C_OK;
 342   }
 343
 344   return res;
 345 }
 346
 347 C_RESULT vp_com_write_udp_socket(vp_com_socket_t* sck, const int8_t* buffer, int32_t* size)
 348 {
 349   C_RESULT res;
 350   int s = (int) sck->priv;
 351   struct sockaddr_in to;
 352
 353   int flags = 0;
 354   if (VP_COM_DONTWAIT == sck->block)
 355     flags |= MSG_DONTWAIT;
 356   else if (VP_COM_WAITALL == sck->block)
 357     flags |= MSG_WAITALL;
 358
 359   if(s >= 0)
 360   {
 361     res = VP_COM_OK;
 362
 363     vp_os_memset( (char*)&to, 0, sizeof(to) );
 364     to.sin_family       = AF_INET;
 365     to.sin_addr.s_addr  = sck->scn;
 366     if ( sck -> remotePort )
 367         to.sin_port     = htons(sck->remotePort);
 368     else
 369         to.sin_port     = htons(sck->port);
 370
 371     *size = sendto( s, (char*)buffer, *size, flags, (struct sockaddr*)&to, sizeof(to) );
 372
 373     if(*size < 0)
 374     {
 375 #ifdef USE_MINGW32
 376
 377       switch( WSAGetLastError() )
 378       {
 379         case WSAEOPNOTSUPP:
 380           PRINT("MSG_NOSIGNAL is not supported on this platform\n");
 381           res = VP_COM_ERROR;
 382           break;
 383
 384         case WSAEINTR:
 385           *size = 0;
 386           break;
 387
 388         case WSAENETDOWN:
 389         case WSAETIMEDOUT:
 390         case WSAECONNRESET:
 391           PRINT("Connection with peer is not enabled\n");
 392           res = VP_COM_ERROR;
 393           break;
 394       }
 395
 396 #else
 397       if( errno == EAGAIN )
 398       {
 399         *size = 0;
 400       }
 401       else
 402       {
 403         switch( errno )
 404         {
 405           case EOPNOTSUPP:
 406             PRINT("MSG_NOSIGNAL is not supported on this platform\n");
 407             break;
 408
 409           case EPIPE:
 410           case ENOTCONN:
 411           case ECONNRESET:
 412             PRINT("Connection with peer is not enabled\n");
 413             break;
 414
 415           default:
 416             PRINT("sendto fails with error: %d - %s\n", errno, strerror(errno));
 417             break;
 418         }
 419
 420         res = VP_COM_ERROR;
 421       }
 422 #endif // USE_MINGW32
 423     }
 424   }
 425   else
 426   {
 427     res = VP_COM_ERROR;
 428   }
 429
 430   return res;
 431 }
 432
 433 C_RESULT vp_com_read_socket(vp_com_socket_t* socket, int8_t* buffer, int32_t* size)
 434 {
 435   C_RESULT res;
 436   int s = (int) socket->priv;
 437
 438   int flags = 0;
 439   if (VP_COM_DONTWAIT == socket->block)
 440     flags |= MSG_DONTWAIT;
 441   else if (VP_COM_WAITALL == socket->block)
 442     flags |= MSG_WAITALL;
 443
 444   if(s >= 0)
 445   {
 446     res = VP_COM_OK;
 447     *size = recv(s, buffer, *size, flags);
 448     if(*size < 0)
 449     {
 450       if( errno == EAGAIN )
 451       {
 452         *size = 0;
 453       }
 454       else
 455       {
 456         res = VP_COM_ERROR;
 457       }
 458     }
 459   }
 460   else
 461   {
 462     res = VP_COM_ERROR;
 463   }
 464
 465   return res;
 466 }
 467
 468 C_RESULT vp_com_write_socket(vp_com_socket_t* socket, const int8_t* buffer, int32_t* size)
 469 {
 470   C_RESULT res;
 471   int s = (int) socket->priv;
 472
 473   int flags = 0;
 474   if (VP_COM_DONTWAIT == socket->block)
 475     flags |= MSG_DONTWAIT;
 476   else if (VP_COM_WAITALL == socket->block)
 477     flags |= MSG_WAITALL;
 478
 479   if(s >= 0)
 480   {
 481     res = VP_COM_OK;
 482     *size = send(s, buffer, *size, flags);
 483     if(*size < 0)
 484     {
 485       if( errno == EAGAIN )
 486       {
 487         *size = 0;
 488       }
 489       else
 490       {
 491         res = VP_COM_ERROR;
 492       }
 493     }
 494   }
 495   else
 496   {
 497     res = VP_COM_ERROR;
 498   }
 499
 500   return res;
 501 }
 502
 503 #endif
 504
 505 /////////////////////////////////////////////////////////////////////////////////////////////////////////////
 506
 507 static vp_os_mutex_t  server_initialisation_mutex;
 508 static vp_os_cond_t   server_initialisation_wait;
 509 static bool_t         server_init_not_finished = FALSE;
 510
 511 C_RESULT vp_com_init_server(void)
 512 {
 513   server_init_not_finished = TRUE;
 514   vp_os_mutex_init(&server_initialisation_mutex);
 515   vp_os_cond_init(&server_initialisation_wait, &server_initialisation_mutex);
 516
 517   return C_OK;
 518 }
 519
 520 C_RESULT vp_com_wait_for_server_up(void)
 521 {
 522   if( server_init_not_finished )
 523   {
 524     vp_os_mutex_lock(&server_initialisation_mutex);
 525     vp_os_cond_wait(&server_initialisation_wait);
 526     vp_os_mutex_unlock(&server_initialisation_mutex);
 527   }
 528
 529   return C_OK;
 530 }
 531
 532 C_RESULT vp_com_timed_wait_for_server_up(uint32_t ms)
 533 {
 534   C_RESULT res = C_OK;
 535
 536   if( server_init_not_finished )
 537   {
 538     vp_os_mutex_lock(&server_initialisation_mutex);
 539     res = vp_os_cond_timed_wait(&server_initialisation_wait, ms);
 540     vp_os_mutex_unlock(&server_initialisation_mutex);
 541   }
 542
 543   return res;
 544 }
 545
 546 extern int32_t vp_com_fill_read_fs(vp_com_socket_t* sockets, int32_t num_sockets, int32_t max, fd_set* read_fs );
 547 extern void vp_com_close_client_sockets(vp_com_socket_t* client_sockets, int32_t num_client_sockets);
 548 extern C_RESULT vp_com_client_open_socket(vp_com_socket_t* server_socket, vp_com_socket_t* client_socket);
 549 extern void vp_com_client_receive( vp_com_socket_t *client_socket );
 550
 551 DEFINE_THREAD_ROUTINE_STACK( vp_com_server, thread_params, VP_COM_THREAD_SERVER_STACK_SIZE )
 552 {
 553 #ifdef CYGPKG_NET
 554   vp_com_socket_t client_sockets[VP_COM_THREAD_NUM_MAX_CLIENTS];
 555   struct timeval tv, *ptv;
 556
 557   // This thread setup connection then loop & wait for a socket event
 558   vp_com_server_thread_param_t* params = (vp_com_server_thread_param_t*) thread_params;
 559
 560   int32_t i, rc, ncs, s, max = 0, num_server_sockets = params->num_servers, num_client_sockets = 0;
 561   vp_com_socket_t* server_sockets = params->servers;
 562   fd_set read_fs;
 563
 564   vp_os_memset( client_sockets, 0, sizeof( client_sockets ));
 565
 566   if(FAILED(vp_com_init(params->com)))
 567   {
 568     DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to init com\n");
 569     vp_com_shutdown(params->com);
 570   }
 571   else if(FAILED(vp_com_local_config(params->com, params->config)))
 572   {
 573     DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to configure com\n");
 574     vp_com_shutdown(params->com);
 575   }
 576   else if(FAILED(vp_com_connect(params->com, params->connection, 1)))
 577   {
 578     DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to connect\n");
 579     vp_com_shutdown(params->com);
 580   }
 581   else
 582   {
 583     vp_os_mutex_lock(&server_initialisation_mutex);
 584     vp_os_cond_signal(&server_initialisation_wait);
 585     vp_os_mutex_unlock(&server_initialisation_mutex);
 586
 587     server_init_not_finished = FALSE;
 588
 589     for( i = 0; i < num_server_sockets; i++ )
 590     {
 591       if(FAILED( vp_com_open_socket(&server_sockets[i], NULL, NULL) ))
 592       {
 593         DEBUG_PRINT_SDK("[VP_COM_SERVER] Unable to open server socket\n");
 594         server_sockets[i].is_disable = TRUE;
 595       }
 596       else
 597       {
 598         listen((int32_t)server_sockets[i].priv, server_sockets[i].queue_length);
 599       }
 600     }
 601
 602     params->run = TRUE;
 603
 604     while( params->run == TRUE )
 605     {
 606       if( params->timer_enable == FALSE || ( params->wait_sec == 0 && params->wait_usec == 0 ) )
 607       {
 608         ptv = NULL;
 609       }
 610       else
 611       {
 612         tv.tv_sec   = params->wait_sec;
 613         tv.tv_usec  = params->wait_usec;
 614         ptv         = &tv;
 615       }
 616
 617       FD_ZERO(&read_fs);
 618       max = vp_com_fill_read_fs( &server_sockets[0], num_server_sockets, 0, &read_fs );
 619       max = vp_com_fill_read_fs( &client_sockets[0], num_client_sockets, max, &read_fs );
 620
 621       rc = select( max + 1, &read_fs, NULL, NULL, ptv );
 622       if( rc == -1 && ( errno == EINTR || errno == EAGAIN ) )
 623         continue;
 624
 625       if( rc == 0 )
 626       {
 627         DEBUG_PRINT_SDK("[VP_COM_SERVER] select timeout\n");
 628
 629         vp_com_close_client_sockets(&client_sockets[0], num_client_sockets);
 630         num_client_sockets = 0;
 631
 632         params->timer_enable  = FALSE;
 633         vp_os_memset( client_sockets, 0, sizeof( client_sockets ));
 634       }
 635
 636       for( i = 0; i < num_server_sockets && rc != 0; i++ )
 637       {
 638         s = (int32_t) server_sockets[i].priv;
 639
 640         if( ( !server_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
 641         {
 642           rc --;
 643
 644           // Recycle previously released sockets
 645           for( ncs = 0; ncs < num_client_sockets && client_sockets[ncs].priv != NULL; ncs++ );
 646
 647           if( ncs < VP_COM_THREAD_NUM_MAX_CLIENTS)
 648           {
 649             if( SUCCEED(vp_com_client_open_socket(&server_sockets[i], &client_sockets[ncs])) && ( ncs == num_client_sockets ) )
 650               num_client_sockets ++;
 651           }
 652         }
 653       }
 654
 655       for( i = 0; i < num_client_sockets && rc != 0; i++ )
 656       {
 657         s = (int32_t) client_sockets[i].priv;
 658         if( ( !client_sockets[i].is_disable ) && FD_ISSET( s, &read_fs) )
 659         {
 660           rc--;
 661
 662           vp_com_client_receive( &client_sockets[i] );
 663         }
 664       }
 665     }
 666
 667     for( i = 0; i < num_server_sockets; i++ )
 668     {
 669       vp_com_close_socket(&server_sockets[i]);
 670     }
 671   }
 672
 673   vp_com_disconnect(params->com);
 674   vp_com_shutdown(params->com);
 675 #endif
 676
 677   THREAD_RETURN( 0 );
 678 }