git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VP_SDK/VP_Com/vp_com_socket_utils.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 int32_t vp_com_fill_read_fs(vp_com_socket_t* sockets, int32_t num_sockets, int32_t max, fd_set* read_fs )
  22 {
  23   while( num_sockets > 0 )
  24   {
  25     if( !sockets->is_disable )
  26     {
  27       int32_t s = (int32_t) sockets->priv;
  28
  29       FD_SET( s, read_fs); // add the socket
  30
  31       if( s > max )
  32         max = s;
  33     }
  34
  35     sockets ++;
  36     num_sockets--;
  37   }
  38
  39   return max;
  40 }
  41
  42 void vp_com_close_client_sockets(vp_com_socket_t* client_sockets, int32_t num_client_sockets)
  43 {
  44   int32_t s;
  45
  46   // Select timed out - We close all sockets because it should mean we lost connection with client
  47   while( num_client_sockets > 0 )
  48   {
  49     if( !client_sockets->is_disable )
  50     {
  51       s = (int32_t) client_sockets->priv;
  52
  53       DEBUG_PRINT_SDK("[VP_COM_SERVER] Closing socket %d\n", (int)s);
  54
  55       client_sockets->select( client_sockets->server,
  56                               client_sockets,
  57                               VP_COM_SOCKET_SELECT_DISABLE,
  58                               (Write) vp_com_write_socket );
  59
  60       if( client_sockets->protocol == VP_COM_TCP )
  61       {
  62         close( s );
  63       }
  64
  65       vp_os_memset( client_sockets, 0, sizeof(vp_com_socket_t) );
  66       client_sockets->is_disable = TRUE;
  67     }
  68
  69     client_sockets++;
  70     num_client_sockets--;
  71   }
  72 }
  73
  74 C_RESULT vp_com_client_open_socket(vp_com_socket_t* server_socket, vp_com_socket_t* client_socket)
  75 {
  76   C_RESULT res;
  77   struct sockaddr_in raddr = { 0 }; // remote address
  78
  79   socklen_t l = sizeof(raddr);
  80   int32_t s = (int32_t) server_socket->priv;
  81
  82   Write write = (Write) (server_socket->protocol == VP_COM_TCP ? vp_com_write_socket : vp_com_write_udp_socket);
  83
  84   vp_os_memcpy( client_socket, server_socket, sizeof(vp_com_socket_t) );
  85
  86   res = server_socket->select( server_socket, client_socket, VP_COM_SOCKET_SELECT_ENABLE, write );
  87
  88   if( SUCCEED(res) )
  89   {
  90     if( server_socket->protocol == VP_COM_TCP )
  91     {
  92       client_socket->priv = (void*)accept( s, (struct sockaddr*)&raddr, &l );
  93     }
  94
  95     DEBUG_PRINT_SDK("[VP_COM_SERVER] Opening socket for server %d\n", (int)s);
  96
  97     client_socket->server  = server_socket;
  98   }
  99   else
 100   {
 101     DEBUG_PRINT_SDK("[VP_COM_SERVER] Failed to open socket for server %d\n", (int)s);
 102     vp_os_memset( client_socket, 0, sizeof(vp_com_socket_t) );
 103   }
 104
 105   return res;
 106 }
 107
 108 void vp_com_client_receive( vp_com_socket_t *client_socket )
 109 {
 110
 111   static int8_t *local_buffer=NULL ;//[VP_COM_THREAD_LOCAL_BUFFER_MAX_SIZE];
 112   static int8_t *new_buffer=NULL;
 113   static int32_t local_buffer_length=0;
 114
 115   struct sockaddr_in from;
 116   socklen_t fromlen;
 117   int32_t s, received,available;
 118
 119   s = (int32_t) client_socket->priv;
 120
 121   fromlen = sizeof(from);
 122
 123   switch(client_socket->protocol)
 124   {
 125   case VP_COM_TCP:
 126           /* TCP : read as much information as we can */
 127
 128           /* Resize the buffer */
 129           if (local_buffer==NULL || local_buffer_length<VP_COM_THREAD_LOCAL_BUFFER_SIZE)          {
 130                   new_buffer=(int8_t *)vp_os_realloc(local_buffer,VP_COM_THREAD_LOCAL_BUFFER_SIZE);
 131                   if (new_buffer==NULL) { PRINT("Not enough memory to store TCP data.\n"); return;  }
 132                   local_buffer = new_buffer;
 133                   local_buffer_length = VP_COM_THREAD_LOCAL_BUFFER_SIZE;
 134           }
 135
 136
 137           /* Fetch data */
 138           received = recvfrom(s, (char*)local_buffer,local_buffer_length, 0, (struct sockaddr*)&from, &fromlen);
 139
 140           /* We close the socket if an error occurred */
 141           if( received <= 0)
 142            {
 143                  PRINT("\nClosing socket on port %i.\n",client_socket->port);
 144              client_socket->select( client_socket->server, client_socket, VP_COM_SOCKET_SELECT_DISABLE, (Write) vp_com_write_socket );
 145              close( s );
 146              vp_os_memset( client_socket, 0, sizeof(vp_com_socket_t) );
 147              client_socket->is_disable = TRUE;
 148            }
 149            /* If some data were received, we pass them to the application.  */
 150            else if( received>=0 && client_socket->read != NULL )
 151            {
 152              client_socket->remotePort = ntohs(from.sin_port);
 153              client_socket->read( (void*) client_socket, local_buffer, &received, (&from)->sin_addr.s_addr );
 154            }
 155
 156           break; // End of TCP processing
 157
 158   case VP_COM_UDP:
 159           /* UDP : read one packet */
 160
 161           /* Query packet size */
 162           available = recvfrom(s, (char*)local_buffer,0, MSG_PEEK|MSG_TRUNC, (struct sockaddr*)&from, &fromlen);
 163
 164           /* Resize the buffer */
 165                   if(available>0){
 166                           if (available>VP_COM_THREAD_LOCAL_BUFFER_MAX_SIZE) {
 167                                   PRINT("UDP packet is bigger than maximum authorized size. Dropping packet.\n"); return;
 168                           }
 169                           if (local_buffer==NULL || local_buffer_length<available)        {
 170                                   new_buffer=(int8_t *)vp_os_realloc(local_buffer,available);
 171                                   if (new_buffer==NULL) {
 172                                           PRINT("UDP packet is bigger than available memory. Dropping packet.\n"); return;
 173                                   }
 174                                   local_buffer = new_buffer; local_buffer_length = available;
 175                   }}
 176                   else { return ; }
 177
 178           /* Fetch data */
 179                 received = recvfrom(s, (char*)local_buffer,local_buffer_length, 0, (struct sockaddr*)&from, &fromlen);
 180
 181
 182           /* Closes the socket if an error occurred on an UDP socket   */
 183                   if( received < 0 )
 184                   {
 185                         PRINT("\nClosing socket on port %i.\n",client_socket->port);
 186                         client_socket->select( client_socket->server, client_socket, VP_COM_SOCKET_SELECT_DISABLE, (Write) vp_com_write_socket );
 187                         close( s );
 188                         vp_os_memset( client_socket, 0, sizeof(vp_com_socket_t) );
 189                         client_socket->is_disable = TRUE;
 190                   }
 191
 192           /* If some data were received, we pass them to the application.  */
 193                   else if( received>=0 && client_socket->read != NULL )
 194                   {
 195             client_socket->remotePort = ntohs(from.sin_port);
 196             client_socket->read( (void*) client_socket, local_buffer, &received, (&from)->sin_addr.s_addr );
 197                   }
 198
 199           break;  // End of UDP processing
 200
 201   default: break;
 202
 203   }
 204
 205   //DEBUG_STEPH  - debugging the fact that an empty UDP packet crashes the Mykonos program
 206
 207   /* PRINT("Received (%i bytes) from (ip %i.%i.%i.%i) on (port %i) - ",
 208                   received,
 209                   ((((struct sockaddr_in*)&from)->sin_addr.s_addr))&0x0FF,
 210                   ((((struct sockaddr_in*)&from)->sin_addr.s_addr)>>8)&0x0FF,
 211                   ((((struct sockaddr_in*)&from)->sin_addr.s_addr)>>16)&0x0FF,
 212                   ((((struct sockaddr_in*)&from)->sin_addr.s_addr)>>24)&0x0FF,
 213                   ntohs(((struct sockaddr_in*)&from)->sin_port)
 214                   );
 215
 216           switch(client_socket->protocol){
 217           case VP_COM_UDP: printf(" UDP"); break;
 218           case VP_COM_TCP: printf(" TCP"); break;
 219           default : printf("unknown\n"); break;
 220           }
 221 */
 222
 223 }