/*
* Display the AP Address if possible
*/
static int
print_ap(int skfd,
const char * ifname,
int format)
{
struct iwreq wrq;
char buffer[64];
/* Get AP Address */
if(iw_get_ext(skfd, ifname, SIOCGIWAP, &wrq) < 0)
return(-1);
/* Print */
iw_ether_ntop((const struct ether_addr *) wrq.u.ap_addr.sa_data, buffer);
switch(format)
{
case FORMAT_SCHEME:
/* I think ':' are not problematic, because Pcmcia scripts
* seem to handle them properly... */
case FORMAT_RAW:
printf("%s\n", buffer);
break;
default:
printf("%-8.16s Access Point/Cell: %s\n", ifname, buffer);
break;
}
return(0);
}
/*
* Input an arbitrary length MAC address and convert to binary.
* Return address size.
*/
int
iw_mac_aton(const char * orig,
unsigned char * mac,
int macmax)
{
const char * p = orig;
int maclen = 0;
/* Loop on all bytes of the string */
while(*p != '\0')
{
int temph;
int templ;
int count;
/* Extract one byte as two chars */
count = sscanf(p, "%1X%1X", &temph, &templ);
if(count != 2)
break; /* Error -> non-hex chars */
/* Output two chars as one byte */
templ |= temph << 4;
mac[maclen++] = (unsigned char) (templ & 0xFF);
/* Check end of string */
p += 2;
if(*p == '\0')
{
#ifdef DEBUG
char buf[20];
iw_ether_ntop((const struct ether_addr *) mac, buf);
fprintf(stderr, "iw_mac_aton(%s): %s\n", orig, buf);
#endif
return(maclen); /* Normal exit */
}
/* Check overflow */
if(maclen >= macmax)
{
#ifdef DEBUG
fprintf(stderr, "iw_mac_aton(%s): trailing junk!\n", orig);
#endif
errno = E2BIG;
return(0); /* Error -> overflow */
}
/* Check separator */
if(*p != ':')
break;
p++;
}
/* Error... */
#ifdef DEBUG
fprintf(stderr, "iw_mac_aton(%s): invalid ether address!\n", orig);
#endif
errno = EINVAL;
return(0);
}
/*
* Display an Wireless Access Point Socket Address in readable format.
* Note : 0x44 is an accident of history, that's what the Orinoco/PrismII
* chipset report, and the driver doesn't filter it.
*/
char *
iw_sawap_ntop(const struct sockaddr * sap,
char * buf)
{
const struct ether_addr ether_zero = {{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }};
const struct ether_addr ether_bcast = {{ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }};
const struct ether_addr ether_hack = {{ 0x44, 0x44, 0x44, 0x44, 0x44, 0x44 }};
const struct ether_addr * ether_wap = (const struct ether_addr *) sap->sa_data;
if(!iw_ether_cmp(ether_wap, ðer_zero))
sprintf(buf, "Not-Associated");
else
if(!iw_ether_cmp(ether_wap, ðer_bcast))
sprintf(buf, "Invalid");
else
if(!iw_ether_cmp(ether_wap, ðer_hack))
sprintf(buf, "None");
else
iw_ether_ntop(ether_wap, buf);
return(buf);
}
/* ------------------------------------------------------------------ WirelessInterface_ScanItem */
static int WirelessInterface_ScanItem(wifacesd* data, iwevent* event, iwrange* range, int has_range)
{
static char buf[128];
memset(data, 0, sizeof(wifacesd));
switch (event->cmd) {
case SIOCGIWAP: {
iw_ether_ntop((const struct ether_addr*)(event->u.ap_addr.sa_data), buf);
data->keys[0] = IWSCAN_KE_BSSID;
data->objs[0] = Py_BuildValue("s", buf);
data->num = 1;
return 1;
}
case SIOCGIWFREQ: {
double freq = iw_freq2float(&(event->u.freq));
int channel;
if (freq <= 14.0)
channel = iw_channel_to_freq((int)(freq), &freq, range);
else
channel = iw_freq_to_channel(freq, range);
iw_print_freq_value(buf, sizeof(buf), freq);
data->keys[0] = IWSCAN_KE_FREQUENCY;
data->keys[1] = IWSCAN_KE_CHANNEL;
data->objs[0] = Py_BuildValue("s", buf);
data->objs[1] = Py_BuildValue("i", channel);
data->num = 2;
return 0;
}
case SIOCGIWMODE: {
data->keys[0] = IWSCAN_KE_MODE;
data->objs[0] = Py_BuildValue("s", iw_operation_mode[event->u.mode]);
data->num = 1;
return 0;
}
case SIOCGIWNAME: {
data->keys[0] = IWSCAN_KE_PROTOCOL;
data->objs[0] = Py_BuildValue("s", event->u.name);
data->num = 1;
return 0;
}
case SIOCGIWESSID: {
memcpy(buf, event->u.essid.pointer, event->u.essid.length);
buf[event->u.essid.length] = 0x0;
data->keys[0] = IWSCAN_KE_ESSID;
data->objs[0] = Py_BuildValue("s", buf);
data->num = 1;
return 0;
}
case SIOCGIWENCODE: {
PyObject* pybool;
if (event->u.data.flags & IW_ENCODE_DISABLED) pybool = Py_False;
else pybool = Py_True;
Py_INCREF(pybool);
data->keys[0] = IWSCAN_KE_ENC;
data->objs[0] = pybool;
data->num = 1;
return 0;
}
case SIOCGIWRATE: {
iw_print_bitrate(buf, sizeof(buf), event->u.bitrate.value);
data->keys[0] = IWSCAN_KE_BITRATE;
data->objs[0] = Py_BuildValue("s", buf);
data->num = 1;
return 0;
}
case IWEVQUAL: {
iw_print_stats(buf, sizeof(buf), &event->u.qual, range, has_range);
data->keys[0] = IWSCAN_KE_QUAL;
data->keys[1] = IWSCAN_KE_STATS;
data->objs[0] = Py_BuildValue("i", event->u.qual.qual);
data->objs[1] = Py_BuildValue("s", buf);
data->num = 2;
return 0;
}
case IWEVGENIE: {
unsigned char* buffer = event->u.data.pointer;
int buflen = event->u.data.length;
int offset = 0;
data->keys[0] = IWSCAN_KE_IE;
//PyObject* ie_list = PyList_New(0);
//data->objs[0] = ie_list;
while (offset <= (buflen - 2)) {
//PyList_Append(ie_list, WirelessInterface_ScanIe(buffer + offset, buflen));
data->objs[0] = WirelessInterface_ScanIe(buffer + offset, buflen);
offset += buffer[offset + 1] + 2;
}
data->num = 1;
return 0;
}
default:
return 0;
}
}
bool WifiStumbler::stumble()
{
int pid;
pid = getpid();
struct iwreq w_request;
w_request.u.data.pointer = (caddr_t)&pid;
w_request.u.data.length = 0;
if (iw_set_ext(wlan_sock_, wlan_if_.c_str(), SIOCSIWSCAN, &w_request) < 0)
{
ROS_ERROR("Couldn't start stumbler.");
return false;
}
timeval time;
time.tv_sec = 0;
time.tv_usec = 200000;
uint8_t *p_buff = NULL;
int buff_size = IW_SCAN_MAX_DATA;
bool is_end = false;
while(!is_end)
{
fd_set fds;
int ret;
FD_ZERO(&fds);
ret = select(0, &fds, NULL, NULL, &time);
if (ret == 0)
{
uint8_t *p_buff2;
while (!is_end)
{
p_buff2 = (uint8_t *)realloc(p_buff, buff_size);
p_buff = p_buff2;
w_request.u.data.pointer = p_buff;
w_request.u.data.flags = 0;
w_request.u.data.length = buff_size;
if (iw_get_ext(wlan_sock_, wlan_if_.c_str(), SIOCGIWSCAN, &w_request) < 0)
{
if (errno == E2BIG && range_.we_version_compiled > 16)
{
if (w_request.u.data.length > buff_size)
buff_size = w_request.u.data.length;
else
buff_size *= 2;
continue;
}
else if (errno == EAGAIN)
{
time.tv_sec = 0;
time.tv_usec = 200000;
break;
}
}
else
is_end = true;
}
}
}
// Put wifi data into ROS message
wifi_tools::AccessPoint ap;
iw_event event;
stream_descr stream;
wifi_stumble_.data.clear();
wifi_stumble_.header.stamp = ros::Time::now();
iw_init_event_stream(&stream, (char *)p_buff, w_request.u.data.length);
is_end = false;
int value = 0;
while(!is_end)
{
value = iw_extract_event_stream(&stream, &event, range_.we_version_compiled);
if(!(value>0))
{
is_end = true;
}
else
{
if(event.cmd == IWEVQUAL)
{
// quality part of statistics
//ROS_INFO("command=IWEVQUAL");
if (event.u.qual.level != 0 || (event.u.qual.updated & (IW_QUAL_DBM | IW_QUAL_RCPI)))
{
ap.noise = event.u.qual.noise;
ap.ss = event.u.qual.level;
}
}
else if(event.cmd == SIOCGIWAP)
{
// get access point MAC addresses
//ROS_INFO("command=SIOCGIWAP");
char mac_buff[1024];
iw_ether_ntop((const struct ether_addr *)&event.u.ap_addr.sa_data,mac_buff);
ap.mac_address = std::string(mac_buff);
ROS_INFO("mac_addr=%s",mac_buff);
}
else if(event.cmd == SIOCGIWESSID)
{
// get ESSID
//ROS_INFO("command=SIOCGIWESSID");
wifi_stumble_.data.push_back(ap);
}
else if(event.cmd == SIOCGIWENCODE)
{
// get encoding token & mode
//ROS_INFO("command=SIOCGIWENCODE");
}
else if(event.cmd == SIOCGIWFREQ)
{
// get channel/frequency (Hz)
//ROS_INFO("command=SIOCGIWFREQ");
}
else if(event.cmd == SIOCGIWRATE)
{
// get default bit rate (bps)
//ROS_INFO("command=SIOCGIWRATE");
}
else if(event.cmd == SIOCGIWMODE)
{
// get operation mode
//ROS_INFO("command=SIOCGIWMODE");
}
else if(event.cmd == IWEVCUSTOM)
{
// Driver specific ascii string
//ROS_INFO("command=IWEVCUSTOM");
}
else if(event.cmd == IWEVGENIE)
{
// Generic IE (WPA, RSN, WMM, ..)
//ROS_INFO("command=IWEVGENIE");
}
else
{
// another command
//ROS_INFO("another command");
}
}
}
checkDuplication();
return true;
}
