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

   1 //#include <network.h>
   2
   3 #include <sys/ioctl.h>
   4 #include <unistd.h>
   5 #include <net/if.h>
   6
   7 #include <sys/types.h>
   8 #include <sys/socket.h>
   9
  10 #include "vp_com_wlc.h"
  11
  12 #include <VP_Com/vp_com.h>
  13 #include <VP_Os/vp_os_malloc.h>
  14 #include <VP_Os/vp_os_print.h>
  15
  16 static int32_t wlsock = 0;
  17 static wl_ioctl_t wldata;
  18 static struct ifreq ifr;
  19 static char buffer[WLC_IOCTL_MAXLEN];
  20
  21 //////////////////////////////////////////////////////////////////////////////////////////////////////
  22
  23 static int vp_wlc_ioctl(int cmd, void *buffer, int length, bool set)
  24 {
  25   if(wlsock < 0)
  26     return -1;
  27
  28   wldata.cmd      = cmd;
  29   wldata.buf      = buffer;
  30   wldata.len      = length;
  31   wldata.set      = set;
  32
  33   int result = ioctl(wlsock, SIOCDEVPRIVATE, &ifr);
  34   if (result<0)
  35   {
  36     perror("ioctl(wlsock, SIOCDEVPRIVATE, &ifr)");
  37   }
  38   return result;
  39 }
  40
  41 static inline int vp_wlc_get(int cmd, void *data, int len)
  42 {
  43   return vp_wlc_ioctl(cmd, data, len, FALSE);
  44 }
  45
  46 static inline int vp_wlc_set(int cmd, void *data, int len)
  47 {
  48   return vp_wlc_ioctl(cmd, data, len, TRUE);
  49 }
  50
  51 static inline int vp_wlc_set_var(void* data, int len)
  52 {
  53   return vp_wlc_set(WLC_SET_VAR, data, len);
  54 }
  55
  56 //////////////////////////////////////////////////////////////////////////////////////////////////////
  57
  58 int32_t vp_wlc_begin(const char* itfname)
  59 {
  60   strcpy(ifr.ifr_name, itfname);
  61   ifr.ifr_data  = (caddr_t) &wldata;
  62
  63   if(wlsock <= 0)
  64     wlsock = socket(AF_INET, SOCK_DGRAM, 0);
  65
  66   return wlsock;
  67 }
  68
  69 int32_t vp_wlc_end(void)
  70 {
  71   if(wlsock > 0)
  72   {
  73     close(wlsock);
  74     wlsock = 0;
  75   }
  76
  77   return wlsock;
  78 }
  79
  80 //////////////////////////////////////////////////////////////////////////////////////////////////////
  81
  82 int32_t vp_wlc_down(void)
  83 {
  84   return vp_wlc_set(WLC_DOWN, NULL, 0);
  85 }
  86
  87 int32_t vp_wlc_up(void)
  88 {
  89   return vp_wlc_set(WLC_UP, NULL, 0);
  90 }
  91
  92 int32_t vp_wlc_get_magic(void)
  93 {
  94   if(vp_wlc_get(WLC_GET_MAGIC, buffer, sizeof(int32_t)) < 0)
  95     return -1;
  96
  97   return *(int32_t*)buffer;
  98 }
  99
 100 int32_t vp_wlc_set_channel(int32_t channel)
 101 {
 102   return vp_wlc_set(WLC_SET_CHANNEL, &channel, sizeof(int32_t));
 103 }
 104
 105 channel_info_t* vp_wlc_get_channel(int32_t* channel)
 106 {
 107   channel_info_t* channel_info = (channel_info_t*) buffer;
 108
 109   *channel = -1;
 110
 111   if(vp_wlc_get(WLC_GET_CHANNEL, buffer, sizeof(channel_info_t)) < 0)
 112     return NULL;
 113
 114   *channel = channel_info->target_channel;
 115
 116   return channel_info;
 117 }
 118
 119 int32_t vp_wlc_set_country(const char* country)
 120 {
 121   char *tmp = (char*)country;
 122   return vp_wlc_set(WLC_SET_COUNTRY, (void*)tmp, strlen(country) + 1);
 123 }
 124
 125 const char* vp_wlc_get_country(void)
 126 {
 127   if(vp_wlc_get(WLC_GET_COUNTRY, buffer, 4) < 0)
 128     return NULL;
 129
 130   return (const char*) buffer;
 131 }
 132
 133 int32_t vp_wlc_set_radio(VP_WLC_RADIO_STATE state)
 134 {
 135   return vp_wlc_set(WLC_SET_RADIO, &state, sizeof(int32_t));
 136 }
 137
 138 int32_t vp_wlc_get_radio(void)
 139 {
 140   int32_t radio_state;
 141
 142   if(vp_wlc_get(WLC_GET_RADIO, buffer, sizeof(int32_t)) < 0)
 143     return -1;
 144
 145   radio_state = *(int32_t*) buffer;
 146   if(radio_state == 0)
 147     return VP_WLC_RADIO_ENABLE;
 148
 149   return VP_WLC_RADIO_DISABLE;
 150 }
 151
 152 int32_t vp_wlc_set_infrastructure(VP_WLC_INFRA_MODE value)
 153 {
 154   return vp_wlc_set(WLC_SET_INFRA, &value, sizeof(int32_t));
 155 }
 156
 157 int32_t vp_wlc_get_infrastructure(void)
 158 {
 159   if(vp_wlc_get(WLC_GET_INFRA, buffer, sizeof(int32_t)) < 0)
 160     return -1;
 161
 162   return *(int32_t*) buffer;
 163 }
 164
 165 int32_t vp_wlc_set_authentication(VP_WLC_AUTH_MODE mode)
 166 {
 167   return vp_wlc_set(WLC_SET_AUTH, &mode, sizeof(int32_t));
 168 }
 169
 170 int32_t vp_wlc_get_authentication(void)
 171 {
 172   if(vp_wlc_get(WLC_GET_AUTH, buffer, sizeof(int32_t)) < 0)
 173     return -1;
 174
 175   return *(int32_t*) buffer;
 176 }
 177
 178 int32_t vp_wlc_set_ssid(const char* name)
 179 {
 180   wlc_ssid_t ssid;
 181
 182   vp_os_memset(&ssid, 0, sizeof(wlc_ssid_t));
 183
 184   if(name)
 185   {
 186     ssid.SSID_len = strlen(name);
 187     strncpy((char *)ssid.SSID, name, ssid.SSID_len);
 188   }
 189
 190   return vp_wlc_set(WLC_SET_SSID, &ssid, sizeof(wlc_ssid_t));
 191 }
 192
 193 wlc_ssid_t* vp_wlc_get_ssid(void)
 194 {
 195   if(vp_wlc_get(WLC_GET_SSID, buffer, sizeof(wlc_ssid_t)) < 0)
 196     return NULL;
 197
 198   return (wlc_ssid_t*) buffer;
 199 }
 200
 201
 202 int32_t vp_wlc_get_bssid(bdaddr_t* bssid)
 203 {
 204   if(vp_wlc_get(WLC_GET_BSSID, buffer, sizeof(bdaddr_t)) < 0)
 205     return -1;
 206
 207   vp_com_copy_address((bdaddr_t*) buffer, bssid);
 208
 209   return 0;
 210 }
 211
 212
 213 int32_t vp_wlc_set_security_mode(int32_t wsec)
 214 {
 215   return vp_wlc_set(WLC_SET_WSEC, &wsec, sizeof(int32_t));
 216 }
 217
 218 int32_t vp_wlc_get_security_mode(void)
 219 {
 220   if(vp_wlc_get(WLC_GET_WSEC, buffer, sizeof(int32_t)) < 0)
 221     return -1;
 222
 223   return *(int32_t*)buffer;
 224 }
 225
 226 int32_t vp_wlc_set_wep128_key(const char* wep_key)
 227 {
 228   wl_wsec_key_t key;
 229
 230   vp_os_memset(&key, 0, sizeof(wl_wsec_key_t));
 231   vp_os_memcpy(&key.data, wep_key, WEP128_KEY_SIZE);
 232
 233   key.len     = WEP128_KEY_SIZE;
 234   key.algo    = CRYPTO_ALGO_WEP128;
 235   key.flags  |= WL_PRIMARY_KEY;
 236
 237   return vp_wlc_set(WLC_SET_KEY, &key, sizeof(key));
 238 }
 239
 240 int32_t vp_wlc_set_mac_mode(int32_t mode)
 241 {
 242   return vp_wlc_set(WLC_SET_MACMODE, &mode, sizeof(int32_t));
 243 }
 244
 245 int32_t vp_wlc_get_mac_mode(void)
 246 {
 247   if( vp_wlc_get(WLC_GET_MACMODE, buffer, sizeof(int32_t)) < 0)
 248     return -1;
 249
 250   return *(int32_t*)buffer;
 251 }
 252
 253 static int32_t vp_wlc_set_roam_trigger(int32_t value)
 254 {
 255   return vp_wlc_set(WLC_SET_ROAM_TRIGGER, &value, sizeof(int32_t));
 256 }
 257
 258 static int32_t vp_wlc_get_roam_trigger(int32_t* value)
 259 {
 260   if( vp_wlc_get(WLC_GET_MACMODE, buffer, sizeof(int32_t)) < 0)
 261     return -1;
 262
 263   *value = *(int32_t*)buffer;
 264
 265   return 0;
 266 }
 267
 268 #define ROAMING_ENABLE_VALUE  0
 269 #define ROAMING_DISABLE_VALUE -99
 270
 271 // Enables roaming
 272 int32_t vp_wlc_enable_roaming(void)
 273 {
 274   return vp_wlc_set_roam_trigger(ROAMING_ENABLE_VALUE);
 275 }
 276
 277 // Disables roaming
 278 int32_t vp_wlc_disable_roaming(void)
 279 {
 280   return vp_wlc_set_roam_trigger(ROAMING_DISABLE_VALUE);
 281 }
 282
 283 // Returns true if roaming is enable. 0 otherwise.
 284 bool_t vp_wlc_roaming_enable(void)
 285 {
 286   bool_t b = FALSE;
 287   int32_t value;
 288
 289   if( vp_wlc_get_roam_trigger(&value) == 0 )
 290   {
 291     switch(value)
 292     {
 293       case ROAMING_ENABLE_VALUE:
 294         b = TRUE;
 295         break;
 296
 297       case ROAMING_DISABLE_VALUE:
 298         b = FALSE;
 299         break;
 300
 301       default:
 302         b = FALSE;
 303         break;
 304     }
 305   }
 306
 307   return b;
 308 }
 309
 310 int32_t vp_wlc_scan(void)
 311 {
 312   int32_t ret = 0;
 313   int32_t ap = 0, oldap = 0;
 314   wl_scan_params_t params;
 315
 316   vp_os_memset(&params, 0, sizeof(params));
 317   vp_os_memset(&params.bssid, 0xff, sizeof(params.bssid));
 318
 319   params.bss_type     = DOT11_BSSTYPE_ANY;
 320   params.scan_type    = -1;
 321   params.nprobes      = -1;
 322   params.active_time  = -1;
 323   params.passive_time = -1;
 324   params.home_time    = -1;
 325
 326   if( vp_wlc_get( WLC_GET_AP, &oldap, sizeof(int32_t) ) < 0 )
 327     ret = -1;
 328
 329   if( oldap > 0 )
 330     vp_wlc_set( WLC_SET_AP, &ap, sizeof(int32_t) );
 331
 332   if( vp_wlc_get(WLC_SCAN, &params, 64) < 0 )
 333     ret = -1;
 334
 335   if( oldap > 0 )
 336     vp_wlc_set( WLC_SET_AP, &oldap, sizeof(oldap) );
 337
 338   return ret;
 339 }
 340
 341 wl_scan_results_t* vp_wlc_get_scan_results(void)
 342 {
 343   wl_scan_results_t* results = (wl_scan_results_t *) buffer;
 344
 345   results->buflen = WLC_IOCTL_MAXLEN - sizeof(wl_scan_results_t);
 346
 347   if( vp_wlc_get(WLC_SCAN_RESULTS, buffer, WLC_IOCTL_MAXLEN) < 0 )
 348     return NULL;
 349
 350   return results;
 351 }
 352
 353 static bool swap = FALSE;
 354 #define dtoh32(i) (swap?bcmswap32(i):i)
 355 #define htod32(i) (swap?bcmswap32(i):i)
 356 #define DIV_QUO(num, div) ((num)/div)  /* Return the quotient of division to avoid floats */
 357 #define DIV_REM(num, div) (((num%div) * 100)/div) /* Return the remainder of division */
 358
 359 /* Byte swap a 32 bit value */
 360 #define BCMSWAP32(val) \
 361         ((uint32)(\
 362                 (((uint32)(val) & (uint32)0x000000ffUL) << 24) | \
 363                 (((uint32)(val) & (uint32)0x0000ff00UL) <<  8) | \
 364                 (((uint32)(val) & (uint32)0x00ff0000UL) >>  8) | \
 365                 (((uint32)(val) & (uint32)0xff000000UL) >> 24)))
 366
 367 static INLINE uint32
 368 bcmswap32(uint32 val)
 369 {
 370         return BCMSWAP32(val);
 371 }
 372
 373 #define QDBM_TABLE_LEN 40
 374 #define QDBM_OFFSET 153
 375
 376 /* Smallest mW value that will round up to the first table entry, QDBM_OFFSET.
 377  * Value is ( mW(QDBM_OFFSET - 1) + mW(QDBM_OFFSET) ) / 2
 378  */
 379 #define QDBM_TABLE_LOW_BOUND 6493 /* QDBM_TABLE_LOW_BOUND */
 380
 381 static const uint16_t nqdBm_to_mW_map[QDBM_TABLE_LEN] = {
 382 /* qdBm:        +0      +1      +2      +3      +4      +5      +6      +7      */
 383 /* 153: */      6683,   7079,   7499,   7943,   8414,   8913,   9441,   10000,
 384 /* 161: */      10593,  11220,  11885,  12589,  13335,  14125,  14962,  15849,
 385 /* 169: */      16788,  17783,  18836,  19953,  21135,  22387,  23714,  25119,
 386 /* 177: */      26607,  28184,  29854,  31623,  33497,  35481,  37584,  39811,
 387 /* 185: */      42170,  44668,  47315,  50119,  53088,  56234,  59566,  63096
 388 };
 389
 390 static uint16_t wl_qdbm_to_mw(uint8_t qdbm)
 391 {
 392   uint32_t factor = 1;
 393   int idx = qdbm - QDBM_OFFSET;
 394
 395   if (idx >= QDBM_TABLE_LEN) {
 396     /* clamp to max uint16 mW value */
 397     return 0xFFFF;
 398   }
 399
 400   /* scale the qdBm index up to the range of the table 0-40
 401    * where an offset of 40 qdBm equals a factor of 10 mW.
 402    */
 403   while (idx < 0) {
 404           idx += 40;
 405           factor *= 10;
 406   }
 407
 408   /* return the mW value scaled down to the correct factor of 10,
 409    * adding in factor/2 to get proper rounding.
 410    */
 411   return ((nqdBm_to_mW_map[idx] + factor/2) / factor);
 412 }
 413
 414 static uint8 wl_mw_to_qdbm(uint16 mw)
 415 {
 416   uint8 qdbm;
 417   int offset;
 418   uint mw_uint = mw;
 419   uint boundary;
 420
 421   /* handle boundary case */
 422   if (mw_uint <= 1)
 423     return 0;
 424
 425   offset = QDBM_OFFSET;
 426
 427   /* move mw into the range of the table */
 428   while (mw_uint < QDBM_TABLE_LOW_BOUND) {
 429     mw_uint *= 10;
 430     offset -= 40;
 431   }
 432
 433   for (qdbm = 0; qdbm < QDBM_TABLE_LEN-1; qdbm++) {
 434     boundary = nqdBm_to_mW_map[qdbm] + (nqdBm_to_mW_map[qdbm+1] - nqdBm_to_mW_map[qdbm])/2;
 435     if (mw_uint < boundary) break;
 436   }
 437
 438   qdbm += (uint8)offset;
 439
 440   return (qdbm);
 441 }
 442
 443 int32_t vp_wlc_get_instant_power(uint32_t *power)
 444 {
 445   int val;
 446   uint8_t buffer[WLC_IOCTL_MAXLEN];
 447   uint32 *int_ptr;
 448   bool override = TRUE;
 449
 450   strcpy((char*)&buffer[0], "qtxpower");
 451   if( vp_wlc_get(WLC_GET_VAR, &buffer[0], strlen((char *)&buffer[0]) + 5) < 0)
 452     return -1;
 453
 454   int_ptr = (uint32 *)&buffer[0];
 455   val = dtoh32(*int_ptr);
 456   override = (bool)(val & WL_TXPWR_OVERRIDE);
 457   val &= ~WL_TXPWR_OVERRIDE;
 458   *power = wl_qdbm_to_mw((uint8)((val<0xff?val:0xff)));
 459
 460   return 0;
 461 }
 462
 463 int32_t vp_wlc_set_instant_power(uint32_t power)
 464 {
 465   unsigned int new_val = 0;
 466   uint8_t pwr_buf[WLC_IOCTL_MAXLEN];
 467   uint8_t *int_ptr;
 468   bool override = TRUE;
 469
 470   strcpy((char *)&pwr_buf[0], "qtxpower");
 471
 472   new_val = wl_mw_to_qdbm((uint16)(power<=0xffff?power:0xffff));
 473
 474   int_ptr = &pwr_buf[strlen((char *)&pwr_buf[0]) + 1];
 475   if (override)
 476     new_val |= WL_TXPWR_OVERRIDE;
 477   new_val = htod32(new_val);
 478   memcpy(int_ptr, (const void *)&new_val, sizeof(new_val));
 479
 480   if( vp_wlc_set(WLC_SET_VAR, &pwr_buf[0], strlen((char *)&pwr_buf[0])+5) < 0 )
 481     return -1;
 482
 483   PRINT("Power set to %d mW\n", power);
 484   return 0;
 485 }