libgfanet/src/gfawifimon.cpp

#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <regex>
#include <poll.h>
#include <signal.h>
#include <errno.h>
#include <limits.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <asm/types.h>
#include <sys/socket.h>
#include <net/if.h>
#include "gfanetmon.h"

/////////////////////////////////////////////////////////////////////////////

#define TRACE(...)					printf(__VA_ARGS__), fflush(stdout)

#define _KILO						1000
#define _MEGA						(_KILO * 1000)
#define _GIGA						(_MEGA * 1000)

#define _WPA_SUPP_CONF_PATH			"/etc/wpa_supplicant/wpa_supplicant.conf"

#define _UNUSED(arg)				(void)arg

#ifndef _countof
#define _countof(a)					(sizeof((a)) / sizeof(*(a)))
#endif	//	_countof

/////////////////////////////////////////////////////////////////////////////

// Scan state and meta-information, used to decode events...
typedef struct iwscan_state
{
	int ap_num;		// Access Point number 1->N
	int val_index;	// Value in table 0->(N-1)
}iwscan_state;

static inline char *iw_saether_ntop(const struct sockaddr *sap, char* bufp)
{
	iw_ether_ntop((const struct ether_addr*)sap->sa_data, bufp);
	return bufp;
}

static const char *	iw_ie_cypher_name[] =
{
    "none",
    "WEP-40",
    "TKIP",
    "WRAP",
    "CCMP",
    "WEP-104"
};
#define	IW_IE_CYPHER_NUM	_countof(iw_ie_cypher_name)

static const char *	iw_ie_cypher_name_wpa[] =
{
    "none",
    "WEP-40",
    "WPA",
    "WRAP",
    "WPA2",
    "WEP-104"
};
#define	IW_IE_CYPHER_WPA_NUM	_countof(iw_ie_cypher_name_wpa)

static bool _FindString(const std::vector<std::string> &arr, const char *pszFind)
{
	for(std::string s : arr)
	{
		if(!s.compare(pszFind))
			return true;
	}
	return false;
}

static bool _PushIfNotExists(std::vector<std::string> &arr, const char *pszNew)
{
	if(!_FindString(arr, pszNew))
	{
		arr.push_back(std::string(pszNew));
		return true;
	}
	return false;
}

static int _GetConnectionInfo(int nIwFd, const char *pszIfName, struct wireless_info *info)
{
    struct iwreq wrq;

    memset((char*) info, 0, sizeof(struct wireless_info));

    /* Get basic information */
    if(::iw_get_basic_config(nIwFd, pszIfName, &(info->b)) < 0)
    {
        /* If no wireless name : no wireless extensions */
        /* But let's check if the interface exists at all */
        struct ifreq ifr;

        strncpy(ifr.ifr_name, pszIfName, IFNAMSIZ);
        if(::ioctl(nIwFd, SIOCGIFFLAGS, &ifr) < 0)
            return -ENODEV;
        else
            return -ENOTSUP;
    }

    /* Get ranges */
    if(::iw_get_range_info(nIwFd, pszIfName, &(info->range)) >= 0)
        info->has_range = 1;

    /* Get AP address */
    if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWAP, &wrq) >= 0)
    {
        info->has_ap_addr = 1;
        memcpy(&(info->ap_addr), &(wrq.u.ap_addr), sizeof (sockaddr));
    }

    /* Get bit rate */
    if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWRATE, &wrq) >= 0)
    {
        info->has_bitrate = 1;
        memcpy(&(info->bitrate), &(wrq.u.bitrate), sizeof(iwparam));
    }

    /* Get Power Management settings */
    wrq.u.power.flags = 0;
    if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWPOWER, &wrq) >= 0)
    {
        info->has_power = 1;
        memcpy(&(info->power), &(wrq.u.power), sizeof(iwparam));
    }

    /* Get stats */
    if(::iw_get_stats(nIwFd, pszIfName, &(info->stats), &info->range, info->has_range) >= 0)
    {
        info->has_stats = 1;
    }

//#ifndef WE_ESSENTIAL
    /* Get NickName */
    wrq.u.essid.pointer = (caddr_t) info->nickname;
    wrq.u.essid.length = IW_ESSID_MAX_SIZE + 2;
    wrq.u.essid.flags = 0;
    if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWNICKN, &wrq) >= 0)
        if(wrq.u.data.length > 1)
            info->has_nickname = 1;

    if((info->has_range) && (info->range.we_version_compiled > 9))
    {
        /* Get Transmit Power */
        if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWTXPOW, &wrq) >= 0)
        {
            info->has_txpower = 1;
            memcpy(&(info->txpower), &(wrq.u.txpower), sizeof(iwparam));
        }
    }

    /* Get sensitivity */
    if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWSENS, &wrq) >= 0)
    {
        info->has_sens = 1;
        memcpy(&(info->sens), &(wrq.u.sens), sizeof(iwparam));
    }

    if((info->has_range) && (info->range.we_version_compiled > 10))
    {
        /* Get retry limit/lifetime */
        if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWRETRY, &wrq) >= 0)
        {
            info->has_retry = 1;
            memcpy(&(info->retry), &(wrq.u.retry), sizeof(iwparam));
        }
    }

    /* Get RTS threshold */
    if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWRTS, &wrq) >= 0)
    {
        info->has_rts = 1;
        memcpy(&(info->rts), &(wrq.u.rts), sizeof(iwparam));
    }

    /* Get fragmentation threshold */
    if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWFRAG, &wrq) >= 0)
    {
        info->has_frag = 1;
        memcpy(&(info->frag), &(wrq.u.frag), sizeof(iwparam));
    }
//#endif	/* WE_ESSENTIAL */

    return 0;
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

CWpaSupplicantConfig::CWpaSupplicantConfig(void)
{
}

CWpaSupplicantConfig::~CWpaSupplicantConfig(void)
{
}
	
bool CWpaSupplicantConfig::WriteConfig(const char *pszEssID, const char *pszPassphrase, const char *pszGoupCiphers, const char *pszPairwCiphers)
{
	std::string sKey;
	
	if(WpaKeyFromPassphrase(pszEssID, pszPassphrase, sKey))
	{
        FILE *fp = fopen(_WPA_SUPP_CONF_PATH, "w");
		
		if(fp)
		{
			fprintf(fp, "ctrl_interface=/var/run/wpa_supplicant\n");
			fprintf(fp, "ap_scan=1\n");
			fprintf(fp, "\n");
			fprintf(fp, "network={\n");
			fprintf(fp, "	ssid=\"%s\"\n", pszEssID);
			fprintf(fp, "	scan_ssid=1\n");
			fprintf(fp, "	proto=RSN\n");
			fprintf(fp, "	key_mgmt=WPA-PSK\n");
			fprintf(fp, "	pairwise=%s\n", pszPairwCiphers);
			fprintf(fp, "	group=%s\n", pszGoupCiphers);
			fprintf(fp, "	psk=%s\n", sKey.c_str());
			fprintf(fp, "}\n");
			fclose(fp);
			return true;
		}
	}

	return false;
}

bool CWpaSupplicantConfig::WpaKeyFromPassphrase(const char *pszEssID, const char *pszPassphrase, std::string &key)
{
	bool bSuccess = false;

	if(pszEssID && *pszEssID && pszPassphrase && *pszPassphrase)
	{
//		int ls = strlen(pszEssID);
		int lp = strlen(pszPassphrase);

		if(lp >= 8 && lp <= 63)
		{
			int c;
			std::string ret;
			char cmd[256];
			sprintf(cmd, "wpa_passphrase \"%s\" \"%s\"", pszEssID, pszPassphrase);
			static const std::string srex = "\\s*psk\\=([0-9a-fA-F]{64})";
			static std::regex rex(srex, std::regex_constants::ECMAScript | std::regex_constants::optimize);
			FILE *fp = popen(cmd, "r");
			
			if(fp)
			{
				std::cmatch res;

				while((c = fgetc(fp)) != EOF)
				{
					ret += (char)c;
				}
				pclose(fp);

				if(regex_search(ret.c_str(), res, rex) && (res.size() == 2) && (res[1].length() == 64))
				{
					key = res[1].str();
					bSuccess = true;
				}
			}
		}
	}

	return bSuccess;
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

class CDynBuf
{
public:
	CDynBuf(size_t nCbInc = 1024) : m_pBuf(nullptr), m_nCbBuf(0), m_nCbInc(nCbInc ? nCbInc : 1024) {
		Realloc();
	}

	virtual ~CDynBuf(void) {
		Free();
	}

public:
	bool Realloc(size_t nCbInc = 0) {
		if(!nCbInc)
			nCbInc = m_nCbInc;
		if((m_pBuf = realloc(m_pBuf, m_nCbBuf + nCbInc))) {
			m_nCbBuf += nCbInc;
			return true;
		}
		return false;
	}
	
	void Free(void) {
		if(m_pBuf) {
			free(m_pBuf);
			m_pBuf = nullptr;
		}
		m_nCbBuf = 0;
	}

	size_t Size(void) const {
		return m_nCbBuf;
	}

public:
	operator void* (void) {
		return m_pBuf;
	}

	operator const void* (void) const {
		return m_pBuf;
	}

private:
	void *m_pBuf;
	size_t m_nCbBuf;
	size_t m_nCbInc;
};

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

CGfaWifiScanResults::CGfaWifiScanResults(void)
{
	Clear();
}

CGfaWifiScanResults::~CGfaWifiScanResults(void)
{
}

uint32_t CGfaWifiScanResults::CompactTrailingZeros(uint32_t v) const
{
	if(v > 0)
	{
		while(!(v % 10))
			v /= 10;
	}
	return v;
}

uint32_t CGfaWifiScanResults::PercentFromDbm(int nDbm)
{
	if(nDbm < -100)
		return 0;
	else if(nDbm > -50)
		return 100;
	else
		return 2 * nDbm + 200;
}

uint32_t CGfaWifiScanResults::BarsFromPercent(uint32_t nPerc)
{
	if(nPerc > 100)
		nPerc = 100;
	return nPerc * 7 / 100;
}

std::string CGfaWifiScanResults::ConcatStrings(std::vector<std::string> &arr)
{
	std::string s;
	size_t cnt = arr.size();
	
	if(cnt > 0)
	{
		s = arr[0];

		for(size_t i = 1; i < arr.size(); ++i)
		{
			s += " ";
			s += arr[i];
		}
	}

	return s;
}

std::string CGfaWifiScanResults::GetBitrateStr(int index) const
{
	uint32_t br = GetBitrate(index);
	if(br > 0)
	{
		char buf[64];
		uint32_t i, f;
		
		if(br >= _GIGA)
		{
			i = br / _GIGA;
			f = CompactTrailingZeros(br % _GIGA);
			sprintf(buf, "%u.%u Gb/s", i, f);
		}
		else if(br >= _MEGA)
		{
			i = br / _MEGA;
			f = CompactTrailingZeros(br % _MEGA);
			sprintf(buf, "%u.%u Mb/s", i, f);
		}
		else if(br >= _KILO)
		{
			i = br / _KILO;
			f = CompactTrailingZeros(br % _KILO);
			sprintf(buf, "%u.%u Kb/s", i, f);
		}
		else
		{
			sprintf(buf, "%u b/s", br);
		}
		return buf;
	}
	return "n/a";
}

std::string CGfaWifiScanResults::GetFreqStr(void) const
{
	double fq = GetFreqHz();
	if(fq > 0.0)
	{
		char buf[64];
		
		if(fq >= _GIGA)
		{
			sprintf(buf, "%.3f GHz", fq / 1000000000.0);
		}
		else if(fq >= _MEGA)
		{
			sprintf(buf, "%.3f MHz", fq / 1000000.0);
		}
		else if(fq >= _KILO)
		{
			sprintf(buf, "%.3f kHz", fq / 1000.0);
		}
		else
		{
			sprintf(buf, "%.3f Hz", fq);
		}
		return buf;
	}
	return "n/a";
}

void CGfaWifiScanResults::SetQualityPercAndBars(const iwqual *qual, const iwrange *range, int has_range)
{
	qualPerc = qualBars = 0;

	if(has_range && ((qual->level != 0) || (qual->updated & (IW_QUAL_DBM | IW_QUAL_RCPI))))
	{
		// Deal with quality : always a relative value
		if(!(qual->updated & IW_QUAL_QUAL_INVALID))
		{
			qualPerc = (uint32_t)qual->qual * 100 / (uint32_t)range->max_qual.qual;
		}

		// Check if the statistics are in RCPI (IEEE 802.11k)
		if(qual->updated & IW_QUAL_RCPI)
		{
			// Deal with signal level in RCPI
			// RCPI = int{(Power in dBm +110)*2} for 0dbm > Power > -110dBm
			if(!(qual->updated & IW_QUAL_LEVEL_INVALID))
			{
				double	rcpilevel = ((double)qual->level / 2.0) - 110.0;
				qualPerc = PercentFromDbm((int)rcpilevel);
			}
		}
		else
		{
			// Check if the statistics are in dBm
			if((qual->updated & IW_QUAL_DBM) || (qual->level > range->max_qual.level))
			{
				// Deal with signal level in dBm  (absolute power measurement)
				if(!(qual->updated & IW_QUAL_LEVEL_INVALID))
				{
					int	dblevel = qual->level;
					// Implement a range for dBm [-192; 63]
					if(qual->level >= 64)
						dblevel -= 0x100;
					qualPerc = PercentFromDbm(dblevel);
				}
			}
			else
			{
				// Deal with signal level as relative value (0 -> max)
				if(!(qual->updated & IW_QUAL_LEVEL_INVALID))
				{
					qualPerc = (uint32_t)qual->level * 100 / (uint32_t)range->max_qual.level;
				}
			}
		}

		qualBars = BarsFromPercent(qualPerc);
	}
}

static const char* iw_print_value_name(unsigned int value, const char *names[], const unsigned int num_names)
{
    if(value >= num_names)
        return "";
    else
        return names[value];
}

void CGfaWifiScanResults::SetWPAStrings(unsigned char *iebuf, int buflen)
{
    int				ielen = iebuf[1] + 2;
    int				offset = 2;	// Skip the IE id, and the length.
    unsigned char	wpa1_oui[3] = {0x00, 0x50, 0xf2};
    unsigned char	wpa2_oui[3] = {0x00, 0x0f, 0xac};
    unsigned char	*wpa_oui;
    int				i;
//    uint16_t		ver = 0;
    uint16_t		cnt = 0;

    if(ielen > buflen)
        ielen = buflen;

    switch(iebuf[0])
    {
    case 0x30:		/* WPA2 */
        /* Check if we have enough data */
        if(ielen < 4)
        {
//            iw_print_ie_unknown(iebuf, buflen);
            return;
        }

        wpa_oui = wpa2_oui;
        break;

    case 0xdd:		/* WPA or else */
        wpa_oui = wpa1_oui;

        /* Not all IEs that start with 0xdd are WPA.
         * So check that the OUI is valid. Note : offset==2 */
        if((ielen < 8) || (memcmp(&iebuf[offset], wpa_oui, 3) != 0) || (iebuf[offset + 3] != 0x01))
            return;

        /* Skip the OUI type */
        offset += 4;
        break;

    default:
        return;
    }

    /* Pick version number (little endian) */
//    ver = iebuf[offset] | (iebuf[offset + 1] << 8);
    offset += 2;

    if(iebuf[0] == 0xdd)
    {
    }

    if(iebuf[0] == 0x30)
    {
//    	m_groupCiphers.clear();
//    	m_pairwCiphers.clear();
    }

    // From here, everything is technically optional.
    // Check if we are done
    if(ielen < (offset + 4))
    {
    	_PushIfNotExists(m_groupCiphers, "TKIP");
    	_PushIfNotExists(m_pairwCiphers, "TKIP");
        return;
    }

    /* Next we have our group cipher. */
    if(memcmp(&iebuf[offset], wpa_oui, 3) != 0)
    {
//        m_groupCiphers.push_back(std::string("Proprietary");
    }
    else
    {
        std::string s = iw_print_value_name(iebuf[offset + 3], iw_ie_cypher_name, IW_IE_CYPHER_NUM);
    	_PushIfNotExists(m_groupCiphers, s.c_str());

        s = iw_print_value_name(iebuf[offset + 3], iw_ie_cypher_name_wpa, IW_IE_CYPHER_WPA_NUM);
    	_PushIfNotExists(m_groupCiphersWPA, s.c_str());
    }
    offset += 4;

    /* Check if we are done */
    if(ielen < (offset + 2))
    {
        /* We don't have a pairwise cipher, or auth method. Assume TKIP. */
    	_PushIfNotExists(m_pairwCiphers, "TKIP");
        return;
    }

    /* Otherwise, we have some number of pairwise ciphers. */
    cnt = iebuf[offset] | (iebuf[offset + 1] << 8);
    offset += 2;

    if(ielen < (offset + 4 * cnt))
        return;

    for(i = 0; i < cnt; i++)
    {
        if(memcmp(&iebuf[offset], wpa_oui, 3) != 0)
        {
//       		m_pairwCiphers.push_back(std::string("Proprietary");
        }
        else
        {
            std::string s = iw_print_value_name(iebuf[offset + 3], iw_ie_cypher_name, IW_IE_CYPHER_NUM);
	    	_PushIfNotExists(m_pairwCiphers, s.c_str());

            s = iw_print_value_name(iebuf[offset + 3], iw_ie_cypher_name_wpa, IW_IE_CYPHER_WPA_NUM);
	    	_PushIfNotExists(m_pairwCiphersWPA, s.c_str());
        }
        offset += 4;
    }
}

void CGfaWifiScanResults::Clear(void)
{
	nwID = 0;
	channel = -1;
	mode = 0;
	encFlags = 0;
	qualPerc = 0;
	qualBars = 0;
	freqHz = 0.0;
	m_bConnected = false;
	memset(&ap_addr, 0, sizeof(ap_addr));
	memset(essid, 0, sizeof(essid));
	memset(qualStr, 0, sizeof(qualStr));

	name.clear();
	itfName.clear();
	m_bitRates.clear();
	m_groupCiphers.clear();
	m_pairwCiphers.clear();
	m_groupCiphersWPA.clear();
	m_pairwCiphersWPA.clear();
}

bool CGfaWifiScanResults::FromWirelessInfo(const char *pszIfName, const struct wireless_info &wli)
{
	Clear();

	if(!wli.has_stats)
		return true;

	SetConnected(true);

    ::iw_print_stats(qualStr, sizeof(qualStr) - 1, &wli.stats.qual, &wli.range, wli.has_range);
	SetQualityPercAndBars(&wli.stats.qual, &wli.range, wli.has_range);

	if(pszIfName && *pszIfName)
		itfName = pszIfName;

	if(wli.has_ap_addr)
		memcpy(&ap_addr, &wli.ap_addr, sizeof(ap_addr));

	if(wli.b.has_nwid)
		nwID = wli.b.nwid.value;

	if(wli.b.has_freq)
	{
		freqHz = wli.b.freq;
		if(wli.has_range)
			channel = iw_freq_to_channel(freqHz, &wli.range);
	}

	if(wli.b.has_mode)
		mode = wli.b.mode;

	if(wli.b.name[0])
	   	name = wli.b.name;

	if(wli.b.essid_len && wli.b.essid[0])
		memcpy(essid, wli.b.essid, (wli.b.essid_len <= IW_ESSID_MAX_SIZE) ? wli.b.essid_len : IW_ESSID_MAX_SIZE);

	if(wli.has_bitrate)
		AddBitrate(wli.bitrate.value);

	return true;
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

CGfaWifiMon::CGfaWifiMon(void) : m_nIwFd(-1)
{
}

CGfaWifiMon::~CGfaWifiMon(void)
{
	IwSocketClose();
}

bool CGfaWifiMon::IwSocketOpen(void)
{
	if(m_nIwFd >= 0)
		IwSocketClose();

	if((m_nIwFd = ::iw_sockets_open()) < 0)
	{
		TRACE("IwSocketOpen: Socket error '%s'!\n", strerror(errno));
	}

	return (m_nIwFd >= 0);
}

void CGfaWifiMon::IwSocketClose(void)
{
	if(m_nIwFd >= 0)
	{
		::iw_sockets_close(m_nIwFd);
		m_nIwFd = -1;
	}
}

int CGfaWifiMon::IwEnum(void)
{
	char *args[] = {(char*)this};
	m_scres.clear();
	::iw_enum_devices(m_nIwFd, &CGfaWifiMon::IwScan, args, 1);
	return 0;
}

void CGfaWifiMon::ProcessConnectedEssid(int nIwFd, const char *pszIfName, std::string &sEssid)
{
	struct iwreq	wrq;
	char			essid[IW_ESSID_MAX_SIZE + 1];	/* ESSID */
	memset(essid, 0, sizeof(essid));

	sEssid.clear();

	wrq.u.essid.pointer = (caddr_t) essid;
	wrq.u.essid.length = IW_ESSID_MAX_SIZE + 1;
	wrq.u.essid.flags = 0;
	if(::iw_get_ext(nIwFd, pszIfName, SIOCGIWESSID, &wrq) < 0)
		return;

	sEssid = essid;
	
	if(sEssid.length() > 0)
	{
		for(CGfaWifiScanResults &scr : m_scres)
		{
			if(	!scr.GetItfName().compare(pszIfName) &&
				!scr.GetESSID().compare(sEssid))
			{
				scr.m_bConnected = true;
			}
		}
	}
}

bool CGfaWifiMon::GetConnectionInfo(const char *pszIfName, CGfaWifiScanResults &wsr)
{
	int nIwFd;

	if((nIwFd = ::iw_sockets_open()) >= 0)
	{
		struct wireless_info wli;
		int ret = _GetConnectionInfo(nIwFd, pszIfName, &wli);
		::iw_sockets_close(nIwFd);
		if(ret == 0)
		{
			return wsr.FromWirelessInfo(pszIfName, wli);
		}
	}

	wsr.Clear();
	return false;
}

int CGfaWifiMon::IwScan(int nIwFd, char *pszIfName, char *args[], int count)
{
	_UNUSED(nIwFd);
	if(count != 1)
		return -1;
	CGfaWifiMon *pThis = (CGfaWifiMon*)args[0];
	return pThis->IwScan(pszIfName);
}

int CGfaWifiMon::IwScan(const char *pszIfName)
{
	CDynBuf dbuf(IW_SCAN_MAX_DATA);
    struct iwreq wrq;
	struct iw_scan_req scanopt;		// Options for 'set'
    struct iw_range	range;
	struct timeval tv;				// Select timeout
    int nRet = 0;
	int timeout = 15000000;			// 15s

	memset(&wrq, 0, sizeof(wrq));
	memset(&scanopt, 0, sizeof(scanopt));
	memset(&range, 0, sizeof(range));

    int has_range = (::iw_get_range_info(m_nIwFd, pszIfName, &range) >= 0);

    // Check if the interface could support scanning.
	if((!has_range) || (range.we_version_compiled < 14))
	{
		TRACE("%-8.16s:  Interface doesn't support scanning.\n", pszIfName);
		return -1;
	}

	// Init timeout value -> 250ms between set and first get
	tv.tv_sec = 0;
	tv.tv_usec = 250000;

	// Initiate Scanning
	if(::iw_set_ext(m_nIwFd, pszIfName, SIOCSIWSCAN, &wrq) < 0)
	{
		if((errno != EPERM) /*|| (scanflags != 0)*/)
		{
			TRACE("%-8.16s  Interface doesn't support scanning : %s\n", pszIfName, strerror(errno));
			return -1;
		}

		// If we don't have the permission to initiate the scan, we may
		// still have permission to read left-over results.
		// But, don't wait !!!
		tv.tv_usec = 0;
	}

	timeout -= tv.tv_usec;

	while(true)
	{
		bool bContinueOuter = false, bBreakOuter  = false;
		fd_set	rfds;		// File descriptors for select
		int		last_fd;	// Last fd
		int		ret;

		// Guess what ? We must re-generate rfds each time
		FD_ZERO(&rfds);
		last_fd = -1;

		// In here, add the rtnetlink fd in the list

		// Wait until something happens
		ret = select(last_fd + 1, &rfds, NULL, NULL, &tv);

		// Check if there was an error
		if(ret < 0)
		{
			if((errno == EAGAIN) || (errno == EINTR) || (errno == ECHILD))
				continue;
			TRACE("Unhandled signal - exiting...\n");
			return -1;
		}

		// Check if there was a timeout
		if(ret == 0)
		{
			do
			{
				wrq.u.data.pointer = dbuf;
				wrq.u.data.length = (unsigned short)dbuf.Size();
				wrq.u.data.flags = 0;

				if(::iw_get_ext(m_nIwFd, pszIfName, SIOCGIWSCAN, &wrq) < 0)
				{
					// Check if buffer was too small (WE-17 only)
					if((errno == E2BIG) && (range.we_version_compiled > 16))
					{
						// Some driver may return very large scan results, either
						// because there are many cells, or because they have many
						// large elements in cells (like IWEVCUSTOM). Most will
						// only need the regular sized buffer. We now use a dynamic
						// allocation of the buffer to satisfy everybody. Of course,
						// as we don't know in advance the size of the array, we try
						// various increasing sizes.

						// Check if the driver gave us any hints.
						if(wrq.u.data.length > dbuf.Size())
						{
							if(dbuf.Realloc((size_t)wrq.u.data.length - dbuf.Size()))
								continue;
							else
							{
								TRACE("Realloc failed: %s\n", strerror(errno));
								return -1;
							}
						}
						else
						{
							if(dbuf.Realloc())
								continue;
							else
							{
								TRACE("Realloc failed: %s\n", strerror(errno));
								return -1;
							}
						}
					}
					else if(errno == EAGAIN)	// Check if results not available yet
					{
						tv.tv_sec = 0;
						tv.tv_usec = 100000;	// Restart timer for only 100ms
						if(timeout > 0)
						{
							timeout -= tv.tv_usec;
							bContinueOuter = true;	// Try again later
						}
						else
						{
							TRACE("Timeout scanning WLAN!\n");
							return -1;
						}
					}
					else
					{
						TRACE("%-8.16s  Failed to read scan data : %s\n", pszIfName, strerror(errno));
						return -2;
					}
				}
	            else
	            {
	            	bBreakOuter = true;
	                break;	// We have the results, go to process them
	            }
			}
			while(false);

			if(bContinueOuter)
			{
				bContinueOuter = false;
				continue;
			}
			
			if(bBreakOuter)
				break;
		}
	}

	if(wrq.u.data.length)
	{
		struct iw_event iwe;
		struct stream_descr stream;
		struct iwscan_state state = { .ap_num = -1, .val_index = 0 };
		int ret;

		::iw_init_event_stream(&stream, (char*)(void*)dbuf, wrq.u.data.length);

        do
        {
            // Extract an event and store it
            if((ret = ::iw_extract_event_stream(&stream, &iwe, range.we_version_compiled)) > 0)
            {
				HandleScanToken(pszIfName, &stream, &iwe, &state, &range, has_range);
			}
        }
        while(ret > 0);
	}

	std::string sConnectedEssid;
	ProcessConnectedEssid(m_nIwFd, pszIfName, sConnectedEssid);

	return nRet;
}

void CGfaWifiMon::HandleScanToken(	const char *pszIfName,
									struct stream_descr *stream,// Stream of events
									struct iw_event *event,		// Extracted token
									struct iwscan_state *state,
									struct iw_range *iw_range,	// Range info
									int has_range)
{
	_UNUSED(stream);
	char buffer[128];
	CGfaWifiScanResults *pscr = nullptr;

	if((state->ap_num >= 0) && ((int)m_scres.size() > state->ap_num))
		pscr = &m_scres[state->ap_num];

    switch(event->cmd)
    {
    case SIOCGIWAP:
		if((int)m_scres.size() <= ++state->ap_num)
		{
			m_scres.emplace_back();
			CGfaWifiScanResults &scr = m_scres.back();
			if(pszIfName && *pszIfName)
				scr.itfName = pszIfName;
			memcpy(&scr.ap_addr, &event->u.ap_addr, sizeof(scr.ap_addr));
	        TRACE("Cell %02d - Address: %s\n", state->ap_num, iw_saether_ntop(&event->u.ap_addr, buffer));
		}
        break;
    case SIOCGIWNWID:
    	if(pscr)
		{
			if(event->u.nwid.disabled)
			{
				pscr->nwID = 0;
				TRACE("  NWID:off/any\n");
			}
			else
			{
				pscr->nwID = event->u.nwid.value;
				TRACE("  NWID:%X\n", event->u.nwid.value);
			}
		}
        break;
    case SIOCGIWFREQ:
    	if(pscr)
		{
			pscr->freqHz = ::iw_freq2float(&(event->u.freq));
			if(has_range)
				pscr->channel = iw_freq_to_channel(pscr->freqHz, iw_range);
		}
        break;
    case SIOCGIWMODE:
    	if(pscr)
		{
			if(event->u.mode >= IW_NUM_OPER_MODE)
				event->u.mode = IW_NUM_OPER_MODE;
			TRACE("  Mode: %s\n", iw_operation_mode[event->u.mode]);
			pscr->mode = event->u.mode;
		}
        break;
    case SIOCGIWNAME:
    	if(pscr)
		{
	    	pscr->name = event->u.name;
	        TRACE("  Protocol:%-1.16s\n", pscr->name.c_str());
	    }
        break;
    case SIOCGIWESSID:
    	if(pscr)
		{
			if((event->u.essid.pointer) && (event->u.essid.length))
				memcpy(pscr->essid, event->u.essid.pointer, (event->u.essid.length <= IW_ESSID_MAX_SIZE) ? event->u.essid.length : IW_ESSID_MAX_SIZE);
			if(event->u.essid.flags)
			{
		        // Does it have an ESSID index?
		        if((event->u.essid.flags & IW_ENCODE_INDEX) > 1)
		            TRACE("  ESSID: \"%s\" [%d]\n", pscr->essid, (event->u.essid.flags & IW_ENCODE_INDEX));
		        else
		            TRACE("  ESSID: \"%s\"\n", pscr->essid);
		    }
		    else
		        TRACE("  ESSID: off/any/hidden\n");
		}
        break;
    case SIOCGIWENCODE:
    	if(pscr)
		{
	        if(event->u.data.flags & IW_ENCODE_DISABLED)
	            TRACE("  Encryption off\n");
			else
	            TRACE("  Encryption on\n");
			pscr->encFlags = event->u.data.flags & IW_ENCODE_FLAGS;
		}
        break;
    case SIOCGIWRATE:
    	if(pscr)
		{
			pscr->AddBitrate(event->u.nwid.value);
		}
        break;
    case SIOCGIWMODUL:
    	if(pscr)
		{
		}
        break;
    case IWEVQUAL:
    	if(pscr)
		{
	        ::iw_print_stats(pscr->qualStr, sizeof(pscr->qualStr) - 1, &event->u.qual, iw_range, has_range);
	        pscr->SetQualityPercAndBars(&event->u.qual, iw_range, has_range);
		}
        break;
    case IWEVGENIE:
    	if(pscr)
		{
		    int offset = 0;
		    unsigned char *buffer = (unsigned char*)event->u.data.pointer;
		    int buflen = event->u.data.length;

		    /* Loop on each IE, each IE is minimum 2 bytes */
		    while(offset <= (buflen - 2))
		    {
		        /* Check IE type */
		        switch(buffer[offset])
		        {
		        case 0xdd:	/* WPA1 (and other) */
		        case 0x30:	/* WPA2 */
		            pscr->SetWPAStrings(buffer + offset, buflen);
		            break;
		        default:
		            break;
		        }
		        /* Skip over this IE to the next one in the list. */
		        offset += buffer[offset + 1] + 2;
		    }
		}
        break;
    case IWEVCUSTOM:
    	if(pscr)
		{
		}
        break;
    default:
        TRACE("Unknown Wireless Token 0x%04X\n", event->cmd);
        break;
	}
}

bool CGfaWifiMon::SetWpaSuppConfig(int cellidx, const char *pszPassphrase)
{
	if((cellidx >= 0) && (cellidx < (int)m_scres.size()))
	{
		std::string ssid = GetESSID(cellidx);
		std::string sgroup = GetWPAGroupCiphers(cellidx);
		std::string spairw = GetWPAPairwiseCiphers(cellidx);
		return CWpaSupplicantConfig::WriteConfig(ssid.c_str(), pszPassphrase, sgroup.c_str(), spairw.c_str());
	}
	return false;
}