git.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/ardrone_tool/Com/config_wifi.c

   1 #include <VP_Os/vp_os_malloc.h>
   2
   3 #include <config.h>
   4 #include <ardrone_tool/Com/config_com.h>
   5
   6 #include <VP_Com/vp_com_socket.h>
   7 #include <VP_Com/vp_com.h>
   8
   9 #include <arpa/inet.h>
  10 #include <sys/types.h>
  11 #include <ifaddrs.h>
  12 #include <errno.h>
  13 #include <stdio.h>
  14
  15 #define BUFFER_SIZE                     1024
  16 #define AUTH_MAX_NUMRETRIES     3
  17 #define AUTH_MSG                        "PARROT AUTH"
  18 #define AUTH_MSG_OK                     "PARROT AUTH OK"
  19
  20 vp_com_t* wifi_com(void)
  21 {
  22   static vp_com_t com = {
  23     VP_COM_WIFI,
  24     FALSE,
  25     0,
  26 #ifdef _WIN32
  27     { 0 },
  28 #else // ! USE_MINGW32
  29     PTHREAD_MUTEX_INITIALIZER,
  30 #endif // ! USE_MINGW32
  31     NULL,
  32     NULL,
  33     0,
  34     NULL,
  35     NULL,
  36     NULL,
  37     NULL,
  38     NULL,
  39     NULL,
  40     NULL,
  41     NULL,
  42     NULL,
  43     NULL,
  44     NULL
  45   };
  46
  47   return &com;
  48 }
  49
  50 C_RESULT wifi_server_auth(struct in_addr *addr)
  51 {
  52         Read read = NULL;
  53         Write write = NULL;
  54         C_RESULT result = C_FAIL;
  55     int numretries = 1;
  56     int8_t recvString[BUFFER_SIZE]; /* Buffer for received string */
  57     int recvStringLen;            /* Length of received string */
  58     struct timeval tv = {0, 500000};
  59     int on=1;
  60     const int8_t msg[] = AUTH_MSG;
  61     struct in_addr to;
  62     struct in_addr from;
  63
  64     vp_com_socket_t socket;
  65
  66     // Initialize sending socket
  67     to.s_addr = htonl(WIFI_BROADCAST_ADDR);
  68         COM_CONFIG_SOCKET_AUTH(&socket, VP_COM_CLIENT, AUTH_PORT, inet_ntoa(to));
  69         socket.protocol = VP_COM_UDP;
  70
  71         if(VP_FAILED(vp_com_init(COM_AUTH())))
  72         {
  73                 printf("Failed to init Authentification\n");
  74                 vp_com_shutdown( COM_AUTH() );
  75                 return C_FAIL;
  76         }
  77
  78         if(VP_FAILED(vp_com_open(COM_AUTH(), &socket, &read, &write)))
  79         {
  80                 printf("Failed to open Authentification\n");
  81                 vp_com_shutdown( COM_AUTH() );
  82                 return C_FAIL;
  83         }
  84
  85     if (setsockopt((int)socket.priv, SOL_SOCKET, SO_BROADCAST,(char *)&on,sizeof(on)) < 0)
  86     {
  87                 printf("Failed to set socket option Authentification\n");
  88         vp_com_close(COM_AUTH(), &socket);
  89                 vp_com_shutdown( COM_AUTH() );
  90         return C_FAIL;
  91     }
  92
  93     if (setsockopt((int)socket.priv, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0)
  94     {
  95                 printf("Failed to set socket option Authentification\n");
  96         vp_com_close(COM_AUTH(), &socket);
  97                 vp_com_shutdown( COM_AUTH() );
  98         return C_FAIL;
  99     }
 100
 101     do
 102     {
 103         int len = strlen((char*)msg);
 104         if(write != NULL)
 105         {
 106                 if (VP_FAILED(write(&socket, msg, &len)))
 107                         {
 108                                 vp_com_close(COM_AUTH(), &socket);
 109                                 vp_com_shutdown( COM_AUTH() );
 110                                 return C_FAIL;
 111                         }
 112         }
 113
 114         printf("Wait authentification\n");
 115         do
 116         {
 117                 if(read != NULL)
 118                 {
 119                 recvStringLen = BUFFER_SIZE;
 120                                 if(VP_FAILED(read(&socket, recvString, &recvStringLen)))
 121                                 {
 122                                         vp_com_close(COM_AUTH(), &socket);
 123                                         vp_com_shutdown( COM_AUTH());
 124                                         return C_FAIL;
 125                                 }
 126                 }
 127
 128                 recvString[recvStringLen] = '\0';
 129         }
 130         while((recvStringLen != 0) && (strcmp((char *)recvString, AUTH_MSG) == 0));
 131     }
 132     while((strcmp((char *)recvString, AUTH_MSG_OK) != 0) && (numretries++ < AUTH_MAX_NUMRETRIES));
 133
 134     if(strcmp((char*)recvString, AUTH_MSG_OK) == 0)
 135     {
 136         from.s_addr = socket.scn;
 137         printf("Authentification ok from %s:%d\n", inet_ntoa(from), socket.port);
 138         memcpy(addr, &from, sizeof(struct in_addr));
 139         result = C_OK;
 140     }
 141
 142     vp_com_close(COM_AUTH(), &socket);
 143         vp_com_shutdown( COM_AUTH() );
 144
 145     return result;
 146 }
 147
 148 vp_com_config_t* wifi_config(void)
 149 {
 150         static vp_com_wifi_config_t config =
 151         {
 152                 { 0 },
 153                 WIFI_MOBILE_IP,
 154                 WIFI_NETMASK,
 155                 WIFI_BROADCAST,
 156                 WIFI_GATEWAY,
 157                 WIFI_SERVER,
 158                 WIFI_INFRASTRUCTURE,
 159                 WIFI_SECURE,
 160                 WIFI_PASSKEY,
 161                 { 0 },
 162         };
 163
 164         struct ifaddrs * ifAddrStructHead = NULL;
 165         struct ifaddrs * ifAddrStruct = NULL;
 166         struct in_addr tmpAddr;
 167         bool_t found = FALSE;
 168
 169         if(strlen(config.itfName) == 0)
 170         {
 171                 getifaddrs(&ifAddrStruct);
 172                 ifAddrStructHead = ifAddrStruct;
 173
 174                 while (!found && (ifAddrStruct != NULL))
 175                 {
 176                         // Looking for WIFI interface's IP address corresponding to WIFI_BASE_ADDR
 177                         if (ifAddrStruct->ifa_addr->sa_family == AF_INET)
 178                         {
 179                                 tmpAddr = ((struct sockaddr_in *)ifAddrStruct->ifa_addr)->sin_addr;
 180                                 if ( (ntohl(tmpAddr.s_addr) & 0xFFFFFF00) == WIFI_BASE_ADDR )
 181                                 {
 182                                         inet_ntop(AF_INET, &tmpAddr, config.localHost, VP_COM_NAME_MAXSIZE);
 183                                         memcpy(config.itfName, ifAddrStruct->ifa_name, strlen(ifAddrStruct->ifa_name));
 184                                         if(VP_FAILED(wifi_server_auth(&tmpAddr)))
 185                                                 tmpAddr.s_addr = htonl ( ntohl(tmpAddr.s_addr) - ( ( ( ntohl(tmpAddr.s_addr) & 0xFF ) - 1 ) % 5 ) );
 186                                         memcpy(config.server, inet_ntoa(tmpAddr), strlen(inet_ntoa(tmpAddr)));
 187                                         tmpAddr = ((struct sockaddr_in *)ifAddrStruct->ifa_netmask)->sin_addr;
 188                                         inet_ntop(AF_INET, &tmpAddr, config.netmask, VP_COM_NAME_MAXSIZE);
 189                                         tmpAddr = ((struct sockaddr_in *)ifAddrStruct->ifa_broadaddr)->sin_addr;
 190                                         inet_ntop(AF_INET, &tmpAddr, config.broadcast, VP_COM_NAME_MAXSIZE);
 191                                         found = TRUE;
 192                                 }
 193                         }
 194                         ifAddrStruct = ifAddrStruct->ifa_next;
 195                 }
 196
 197                 if (ifAddrStructHead != NULL)
 198                 {
 199                   freeifaddrs(ifAddrStructHead);
 200                 }
 201         }
 202
 203         return (vp_com_config_t*)&config;
 204 }
 205
 206 vp_com_connection_t* wifi_connection(void)
 207 {
 208   static vp_com_wifi_connection_t connection = {
 209     0,
 210     WIFI_NETWORK_NAME
 211   };
 212
 213   return (vp_com_connection_t*) &connection;
 214 }
 215
 216 void wifi_config_socket(vp_com_socket_t* socket, VP_COM_SOCKET_TYPE type, int32_t port, const char* serverhost)
 217 {
 218   vp_os_memset(socket, 0, sizeof(vp_com_socket_t));
 219
 220   socket->type           = type;
 221   socket->protocol       = VP_COM_TCP;
 222   socket->port           = port;
 223
 224   if(serverhost && socket->type == VP_COM_CLIENT)
 225     strcpy(socket->serverHost, serverhost);
 226 }