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

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