dbus-handler.c

Go to the documentation of this file.

#include <stdio.h>
#include <stdlib.h>
#include <glib.h>
#include <net/ethernet.h>
#include <linux/socket.h>
#include <gconf/gconf-client.h>
#include <osso-ic-dbus.h>

#define DBUS_API_SUBJECT_TO_CHANGE
#include <dbus/dbus.h>
#include <eap-dbus.h>
#include <wlancond-dbus.h>
#ifdef USE_MCE_MODE
#include <mce/dbus-names.h>
#endif
#include "log.h"
#include "dbus.h"
#include "dbus-helper.h"
#include "dbus-handler.h"
#include "common.h"
#include "wpa.h"

#define WLANCOND_SHUTDOWN_DELAY 4 //4s
#define WLANCOND_CONNECT_TIMEOUT 10 //10s
#define WLANCOND_SCAN_TIMEOUT 8 //8s
#define WLANCOND_RESCAN_DELAY 1

/* Saved DBUS names */
static char *scan_name_cache = NULL;
static char *connect_name_cache = NULL;

static int wlan_socket = -1;

struct wlan_status_t wlan_status;

static gboolean _flight_mode = FALSE;
static gboolean power_down_after_scan = FALSE;
static dbus_bool_t saved_inactivity = FALSE;

/* Timer IDs */
static guint wlan_if_down_timer_id = 0;
static guint wlan_connect_timer_id = 0;

/* This is the desired powersave and can be set from DBUS when connecting */
static guint powersave = WLANCOND_SHORT_CAM;

#define WLAN_PREFIX_STR "wlan"

int socket_open(void) 
{
        if (wlan_socket > 0)
                return wlan_socket;
        
        wlan_socket = socket(AF_INET, SOCK_DGRAM, 0);

        if (wlan_socket < 0)
                die("socket() failed");
        
        return wlan_socket;
}
void init_iwreq(struct iwreq* req) 
{
        memset(req, 0, sizeof(struct iwreq));
        strncpy(req->ifr_name, wlan_status.ifname, IFNAMSIZ);
}
static int get_own_mac(void) 
{
        struct ifreq req;
        
        memset(&req , 0, sizeof(req));
        memcpy(req.ifr_name, wlan_status.ifname, IFNAMSIZ);
        
        if (ioctl(socket_open(), SIOCGIFHWADDR, &req) < 0)
        {
                return -1;
        }
        
        memcpy(wlan_status.own_mac, req.ifr_hwaddr.sa_data, ETH_ALEN);
        
        return 0;
}

static gint get_gconf_int(const gchar* path) 
{
        GConfClient *client;
        GConfValue *gconf_value;
        GError *error = NULL;
        gint value = -1;
 
        client = gconf_client_get_default();
        if (client == NULL) {
                return -1;
        }

        gconf_value = gconf_client_get(client, path, &error); 

        g_object_unref(client);

        if (error != NULL) {
                DLOG_ERR("Could not get setting:%s, error:%s", path, 
                         error->message);
                
                g_clear_error(&error);
                return -1;
        }
        
        if (gconf_value == NULL) {
                return -1;
        }
        if (gconf_value->type == GCONF_VALUE_INT) {
                value = gconf_value_get_int(gconf_value);
                DLOG_DEBUG("User selected value: %d", value);
        }
        
        gconf_value_free(gconf_value);
        return value;
}
void update_own_ie(unsigned char* wpa_ie, guint wpa_ie_len) 
{
        g_free(wlan_status.wpa_ie.ie);
        wlan_status.wpa_ie.ie = wpa_ie;
        wlan_status.wpa_ie.ie_len = wpa_ie_len;
}
guint get_encryption_info(void) 
{
        guint auth_status = 0;

        if (wlan_status.pairwise_cipher & CIPHER_SUITE_CCMP) {
                auth_status |= WLANCOND_WPA_AES;
        } else if (wlan_status.pairwise_cipher & CIPHER_SUITE_TKIP) {
                auth_status |= WLANCOND_WPA_TKIP;
        } else if (wlan_status.pairwise_cipher & CIPHER_SUITE_WEP40 ||
                   wlan_status.pairwise_cipher & CIPHER_SUITE_WEP104) {
                auth_status |= WLANCOND_WEP;
        } else {
                auth_status |= WLANCOND_OPEN;
        }
        return auth_status;
}

int init_dbus_handler(void)
{
        if (get_own_mac() < 0) {
                DLOG_ERR("Could not get own MAC address");
                return -1; 
        }

        return 0;
}
int clean_dbus_handler(void)
{
        if (wlan_socket > 0)
                close(wlan_socket);
        return 0;
}
void mode_change(const char *mode) {

        DLOG_DEBUG("WLAN flight mode changed to \"%s\"", mode);
        
        if (g_str_equal(mode, "flight")) {
                set_wlan_state(WLAN_NOT_INITIALIZED, DISCONNECTED_SIGNAL, FORCE_YES);
                _flight_mode = TRUE;
        }
        else if (g_str_equal(mode, "normal")) {
                _flight_mode = FALSE;
        }
        else {
                DLOG_ERR("Invalid mode \"%s\" passed to mode_change()", mode);
        }
}
#ifdef USE_MCE_MODE
static DBusHandlerResult mode_change_dbus(DBusMessage *message) {

        char *mode;
        
        if (!dbus_message_get_args(message, NULL,
                                   DBUS_TYPE_STRING, &mode,
                                   DBUS_TYPE_INVALID)) {
                DLOG_ERR("Invalid arguments for device_mode_ind signal");
                return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
        }
        
        mode_change(mode);
        
        return DBUS_HANDLER_RESULT_HANDLED;
}
#ifdef ACTIVITY_CHECK
static DBusHandlerResult activity_check_dbus(DBusMessage *message) {
        
        if (!dbus_message_get_args(message, NULL,
                                   DBUS_TYPE_BOOLEAN, &saved_inactivity,
                                   DBUS_TYPE_INVALID)) {
                DLOG_ERR("Invalid arguments for device_activity signal");
                return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
        }
        activity_check(saved_inactivity);
        
        return DBUS_HANDLER_RESULT_HANDLED;
}
#endif
#endif

#ifdef ACTIVITY_CHECK

void activity_check(dbus_bool_t inactivity) {

        int sock;
        
        if (get_wlan_state() != WLAN_CONNECTED) {
                return;
        }
        
        sock = socket_open();

        if (inactivity == FALSE) {
                //DLOG_DEBUG("WLAN activity mode changed to active");
        } else {
                //DLOG_DEBUG("WLAN activity mode changed to inactive");
        }

        set_power_state(powersave, sock);     
}

static gboolean get_inactivity_status(void) 
{
        return saved_inactivity;
}
#endif

static DBusHandlerResult icd_check_signal_dbus(DBusMessage *message) {

        char *icd_name;
        char *icd_type;
        char *icd_state;
        char *icd_disconnect_reason;
        DBusError dbus_error;
        
        if ((get_wlan_state() != WLAN_CONNECTED &&
            get_wlan_state() != WLAN_NO_ADDRESS) ||
            get_mode() != WLANCOND_INFRA) {
                return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
        }

        dbus_error_init(&dbus_error);
        if (!dbus_message_get_args(message, &dbus_error,
                                   DBUS_TYPE_STRING, &icd_name,
                                   DBUS_TYPE_STRING, &icd_type,
                                   DBUS_TYPE_STRING, &icd_state,
                                   DBUS_TYPE_STRING, &icd_disconnect_reason,
                                   DBUS_TYPE_INVALID ) )
        {
                DLOG_ERR("Could not get args from signal, '%s'",
                         dbus_error.message);
                dbus_error_free(&dbus_error);
                return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
        }
        
        if (icd_state != NULL && strncmp(icd_state, "CONNECTED", 9) == 0) {

                set_wlan_state(WLAN_CONNECTED, NO_SIGNAL, FORCE_NO);

                DLOG_DEBUG("Going to power save");
                        
                if (set_power_state(powersave, socket_open()) == FALSE) {
                        DLOG_ERR("Failed to set power save");
                }

        }

        //DLOG_DEBUG("Handled icd signal, icd_state:%s", icd_state);
        
        return DBUS_HANDLER_RESULT_HANDLED;
}

static gboolean in_flight_mode(void) {
        return _flight_mode;
}

void set_wlan_signal(gboolean high_or_low) 
{
        if (high_or_low == WLANCOND_HIGH) {
                wlan_status.signal = WLANCOND_HIGH;
                remove_roam_scan_timer();
        } else {
                wlan_status.signal = WLANCOND_LOW;
        }
}

void remove_roam_scan_timer(void) 
{
        if (wlan_status.roam_scan_id) {
                g_source_remove(wlan_status.roam_scan_id);
                wlan_status.roam_scan_id = 0;
        }
}

void remove_connect_timer(void) 
{
        if (wlan_connect_timer_id) {
                g_source_remove(wlan_connect_timer_id);
                wlan_connect_timer_id = 0;
        }
}

static void remove_wlan_if_timer(void) 
{
        // Remove shutdown timer if exists
        if (wlan_if_down_timer_id) {        
                g_source_remove(wlan_if_down_timer_id);
                wlan_if_down_timer_id = 0;
        }
}

void remove_scan_timer(void) 
{
        if (wlan_status.scan_id) {
                g_source_remove(wlan_status.scan_id);
                wlan_status.scan_id = 0;
        }
}

static gboolean wlan_connect_timer_cb(void* data) 
{
        if (wlan_connect_timer_id && get_wlan_state() == 
            WLAN_INITIALIZED_FOR_CONNECTION) {
                
                wlan_connect_timer_id = 0;
                
                DLOG_DEBUG("Association timeout, try: %d", 
                           wlan_status.retry_count);
                
                /* Remove the failed AP from the list */
                remove_from_roam_cache(wlan_status.conn.bssid);
                
                /* Set BSSID to 0 */
                memset(wlan_status.conn.bssid, 0, ETH_ALEN);
                
                if (find_connection_and_associate(wlan_status.roam_cache, 
                                                  FALSE, FALSE) == 0)
                        return FALSE;
                
                /* Try to scan again if retries left */
                if (++wlan_status.retry_count < WLANCOND_MAX_SCAN_TRIES) {
                        if (scan(wlan_status.conn.ssid, 
                                 wlan_status.conn.ssid_len, FALSE) == 0) {
                                return FALSE;
                        }
                }
                
                set_wlan_state(WLAN_NOT_INITIALIZED, DISCONNECTED_SIGNAL, 
                               FORCE_YES);
                return FALSE;
        }
        
        wlan_connect_timer_id = 0;        
        
        //DLOG_DEBUG("Association OK");        

        return FALSE;
}
static gboolean wlan_scan_cb(void* data) 
{
        
        wlan_status.scan_id = 0;

        DLOG_ERR("Scan failed, should not happen!");

        set_scan_state(SCAN_NOT_ACTIVE);
        
        if (get_wlan_state() == WLAN_INITIALIZED_FOR_CONNECTION) {
                set_wlan_state(WLAN_NOT_INITIALIZED, DISCONNECTED_SIGNAL, 
                               FORCE_YES);
        }
        
        return FALSE;
}
static gboolean wlan_if_down_cb(void* data) 
{
        
        wlan_if_down_timer_id = 0;
        
        if (get_wlan_state() == WLAN_NOT_INITIALIZED) {
                DLOG_DEBUG("Delayed shutdown occurred");
                set_wlan_state(WLAN_NOT_INITIALIZED, NO_SIGNAL, FORCE_YES);
                return FALSE;
        }
        
        //DLOG_DEBUG("Delayed shutdown did not happen");        

        return FALSE;
}
int mlme_command(guchar* addr, guint16 cmd, guint16 reason_code)
{
        struct iwreq req;
        struct iw_mlme mlme;

        init_iwreq(&req);

        DLOG_DEBUG("%s", cmd==IW_MLME_DEAUTH?"Deauthenticating":
                   "Disassociating");

        memset(&mlme, 0, sizeof(mlme));
        
        mlme.cmd = cmd;
        mlme.reason_code = reason_code;
        mlme.addr.sa_family = ARPHRD_ETHER;
        memcpy(mlme.addr.sa_data, addr, ETH_ALEN);
        
        req.u.data.pointer = (caddr_t) &mlme;
        req.u.data.length = sizeof(mlme);

        if (ioctl(socket_open(), SIOCSIWMLME, &req) < 0) {
                DLOG_ERR("Failed to run MLME command");
                return -1;
        }

        return 0;
}
static int set_mode(guint32 mode) 
{
        struct iwreq req;

        init_iwreq(&req);

        switch (mode) {
            case WLANCOND_ADHOC:
                    req.u.mode = IW_MODE_ADHOC;
                    DLOG_DEBUG("Setting mode: adhoc");
                    break;
            case WLANCOND_INFRA:
                    req.u.mode = IW_MODE_INFRA;
                    DLOG_DEBUG("Setting mode: infra");
                    break;
            default:
                    DLOG_ERR("Operating mode undefined\n");
                    return -1;
        }                               
        
        if (ioctl(socket_open(), SIOCSIWMODE, &req) < 0) {
                DLOG_ERR("Operating mode setting failed\n");
                return -1;
        }
        return 0;
}
static int set_wep_keys(struct connect_params_t *conn) 
{
        struct iwreq req;
        guint nbr_of_keys = 0;
        int sock;
        guint i;
        
        sock = socket_open();
        
        /* Encryption keys */
        for (i=0;i<4;i++) {
                if(conn->key_len[i] == 0) {
                        continue;
                } else {
                        if (conn->key_len[i] < WLANCOND_MIN_KEY_LEN || 
                            conn->key_len[i] > WLANCOND_MAX_KEY_LEN) {
                                return -1;
                        }
                        
                        init_iwreq(&req);
                        req.u.data.length = conn->key_len[i];
                        req.u.data.pointer = (caddr_t) &conn->key[i][0];
                        req.u.data.flags |= IW_ENCODE_RESTRICTED;
                        req.u.encoding.flags = i+1;
                        nbr_of_keys++;
                }
#define DEBUG_KEY
#ifdef DEBUG_KEY
                int k;
                unsigned char* p = &conn->key[i][0];
                for (k=0;k<conn->key_len[i];k++) {
                        DLOG_DEBUG("Key %d, 0x%02x\n", i, *(p+k));
                }
#endif
                if (ioctl(sock, SIOCSIWENCODE, &req) < 0) {
                        DLOG_ERR("Set encode failed\n");
                        return -1;
                }
                
        }
        
        if (nbr_of_keys) {
                
                DLOG_DEBUG("Default key: %d\n", conn->default_key);

                init_iwreq(&req);
                
                /* Set the default key */
                req.u.encoding.flags = conn->default_key;
                
                if (ioctl(sock, SIOCSIWENCODE, &req) < 0) {
                        DLOG_ERR("Set encode failed\n");
                        return -1;
                }
        }

        return 0;
}

int set_essid(char* essid, int essid_len) 
{
        struct iwreq req;

        DLOG_DEBUG("Setting SSID: %s", essid);

        init_iwreq(&req);
        
        req.u.essid.pointer = (caddr_t)essid;
        req.u.essid.length = essid_len -1; // Remove NULL termination
        req.u.essid.flags = 1;

        if (ioctl(socket_open(), SIOCSIWESSID, &req) < 0) {
                DLOG_ERR("set ESSID failed");
                return -1;
        }
        return 0;
}
int set_bssid(unsigned char *bssid)
{
        struct iwreq req;

        print_mac("Setting BSSID", bssid);
        
        init_iwreq(&req);

        req.u.ap_addr.sa_family = ARPHRD_ETHER;
        
        memcpy(req.u.ap_addr.sa_data, bssid, ETH_ALEN);
        
        if (ioctl(socket_open(), SIOCSIWAP, &req) < 0) {
                DLOG_ERR("Failed to set BSSID");
                return -1;
        }
        return 0;
}

void set_wlan_state(int new_state, int send_signal, force_t force) 
{
        const char *status_table[] = 
                {
                        (char*)"WLAN_NOT_INITIALIZED",
                        (char*)"WLAN_INITIALIZED",
                        (char*)"WLAN_INITIALIZED_FOR_SCAN",
                        (char*)"WLAN_INITIALIZED_FOR_CONNECTION",
                        (char*)"WLAN_NO_ADDRESS",
                        (char*)"WLAN_CONNECTED"
                };

        switch (new_state) {
                
            case WLAN_NOT_INITIALIZED:

                    if (wlan_status.state == WLAN_CONNECTED ||
                        wlan_status.state == WLAN_NO_ADDRESS) {
                            /* Disconnect previous connection */
                            mlme_command(wlan_status.conn.bssid,
                                         IW_MLME_DEAUTH,  
                                         WLANCOND_REASON_LEAVING);
                            set_bssid(NULL_BSSID);
                            set_essid((char*)"", 1);
                    }

                    set_scan_state(SCAN_NOT_ACTIVE);
                    
                    if (get_wlan_state() != WLAN_NOT_INITIALIZED &&
                        get_wlan_state() != WLAN_INITIALIZED_FOR_SCAN)
                            clear_wpa_mode();
                    
                    wlan_status.retry_count = 0;
                    wlan_status.roam_scan = WLANCOND_MIN_ROAM_SCAN_INTERVAL;
                    wlan_status.ip_ok = FALSE;                    

                    /* Remove association timer */
                    remove_connect_timer();

                    /* Remove scan timer */
                    remove_scan_timer();
                    
                    set_wlan_signal(WLANCOND_HIGH);
                    
                    // Remove shutdown timer if exists
                    remove_wlan_if_timer();

                    if (force == FORCE_YES) {
                            
                            clean_roam_cache();
                            
                            if (set_interface_state(socket_open(), CLEAR, 
                                                    IFF_UP)<0) {
                                    DLOG_ERR("Could not set interface down");
                                    return;
                            }
                            
                    } else {
                            
                            DLOG_DEBUG("Delaying interface shutdown");

                            /* Removed, does not reduce power consumption
                               when not connected */
                            //set_power_state(WLANCOND_FULL_POWERSAVE, sock);
                            
                            wlan_if_down_timer_id = g_timeout_add_seconds(
                                    WLANCOND_SHUTDOWN_DELAY,
                                    wlan_if_down_cb,
                                    NULL); 
                            
                    }
                
                    if (send_signal == DISCONNECTED_SIGNAL)
                            disconnected_signal();       

                    break;
            case WLAN_INITIALIZED_FOR_CONNECTION:
                    /* 
                       Set BSSID to 0, this state happens e.g when we drop from
                       the network 
                    */
                    memset(wlan_status.conn.bssid, 0, ETH_ALEN);

                    /* Remove association timer */
                    remove_connect_timer();
                    break;
            case WLAN_CONNECTED:
                    wlan_status.ip_ok = TRUE;
                    break;
            default:
                    break;
        }
        DLOG_DEBUG("Wlancond state change, old_state: %s, new_state: %s", 
                   status_table[wlan_status.state], status_table[new_state]);
        wlan_status.state = new_state;
}

guint get_wlan_state(void) 
{
        return wlan_status.state;
}

void set_scan_state(guint new_state) 
{
        if (wlan_status.scan == new_state) {
                return;
        }
        if (new_state == SCAN_NOT_ACTIVE && wlan_status.scan == SCAN_ACTIVE && 
            scan_name_cache != NULL) {
                remove_scan_timer();
                DLOG_DEBUG("Sending empty results");
                send_dbus_scan_results(NULL, scan_name_cache, 0);
                g_free(scan_name_cache);
                scan_name_cache = NULL;
        }
        
        DLOG_DEBUG("Wlancond scan change, old_state: %s, new_state: %s", 
                   wlan_status.scan==SCAN_NOT_ACTIVE ? "SCAN_IDLE":"SCANNING",
                   new_state == SCAN_NOT_ACTIVE ? "SCAN_IDLE":"SCANNING");
        wlan_status.scan = new_state;
}

guint get_scan_state(void) 
{
        return wlan_status.scan;
}
guint get_mode(void) 
{
        return wlan_status.conn.mode;
}

gboolean set_power_state(guint new_state, int sock) 
{
        struct iwreq req;
        gint sleep_timeout;
        
        if (new_state == WLANCOND_SHORT_CAM && get_inactivity_status() == TRUE)
                new_state = WLANCOND_VERY_SHORT_CAM;
        
        if (wlan_status.power == new_state) {
                return TRUE;
        }

        init_iwreq(&req);

        switch (new_state) {
            case WLANCOND_POWER_ON:
                    req.u.power.disabled = 1;
                    break;
            case WLANCOND_LONG_CAM:
                    req.u.power.flags = IW_POWER_TIMEOUT | IW_POWER_ALL_R;
                    req.u.power.value = WLANCOND_LONG_CAM_TIMEOUT;
                    break;
            case WLANCOND_SHORT_CAM:
                    req.u.power.flags = IW_POWER_TIMEOUT | IW_POWER_ALL_R;
                    sleep_timeout = get_gconf_int(SLEEP_GCONF_PATH);
                    if (sleep_timeout < 0)
                            sleep_timeout = WLANCOND_DEFAULT_SLEEP_TIMEOUT;
                    req.u.power.value = sleep_timeout;
                    break;
            case WLANCOND_VERY_SHORT_CAM:
                    req.u.power.flags = IW_POWER_TIMEOUT | IW_POWER_ALL_R ;
                    sleep_timeout = get_gconf_int(INACTIVE_SLEEP_GCONF_PATH);
                    if (sleep_timeout < 0)
                            sleep_timeout = WLANCOND_VERY_SHORT_CAM_TIMEOUT;
                    req.u.power.value = sleep_timeout;
                    break;
            default:
                    req.u.power.flags = IW_POWER_ALL_R;
                    break;
        }
        
        if (ioctl(sock, SIOCSIWPOWER, &req) < 0) {
                DLOG_ERR("set power failed, state:%d", new_state);
                return FALSE;
        }

        if (req.u.power.value) {
                DLOG_DEBUG("CAM timeout: %d ms", req.u.power.value / 1000);
        }

        wlan_status.power = new_state;

        return TRUE;
}


static int init_if(int sock) 
{
        int previous_state = get_wlan_state();
        
        if (previous_state == WLAN_NOT_INITIALIZED) {
                /* Check if interface is still up from delayed shutdown */
                if (wlan_if_down_timer_id == 0) {
                        if (set_interface_state(sock, SET, 
                                                IFF_UP | IFF_RUNNING) < 0) {
                                return -1;
                        }
                }
                set_power_state(WLANCOND_POWER_ON, sock);
                set_wlan_state(WLAN_INITIALIZED, NO_SIGNAL, FORCE_YES);
        }
        
        return previous_state;
}

static int set_we_name(int sock, char *name, char *args[], int count)
{
        struct iwreq req;
        
        memset(&req, 0, sizeof(req));
        strncpy(req.ifr_name, name, IFNAMSIZ);
        
        if (ioctl(sock, SIOCGIWNAME, &req) < 0) {
                //DLOG_DEBUG("Ifname %s does not support wireless extensions\n",        name);
        } else {
                //DLOG_DEBUG("Found interface %s", name);
                if (g_str_has_prefix(name, WLAN_PREFIX_STR)) {
                        DLOG_DEBUG("Found WLAN interface %s", name);
                        memcpy(&wlan_status.ifname, name, IFNAMSIZ);
                        wlan_status.ifname[IFNAMSIZ] = '\0';
                }
        }
        
        return 0;
}
int get_we_device_name(void) 
{
        memset(&wlan_status, 0, sizeof(wlan_status));

        iw_enum_devices(socket_open(), &set_we_name, NULL, 0);

        if (strnlen(wlan_status.ifname, IFNAMSIZ) < 2)
                return -1;
        
        return 0;
}
int set_interface_state(int sock, int dir, short flags) 
{
        struct ifreq ifr;

        memset(&ifr, 0, sizeof(ifr));
        
        strncpy(ifr.ifr_name, wlan_status.ifname, IFNAMSIZ);
        
        if (ioctl(sock, SIOCGIFFLAGS, &ifr) < 0) {
                DLOG_ERR("Could not get interface %s flags\n", 
                         wlan_status.ifname); 
                return -1;
        }
        if (dir == SET) {
                ifr.ifr_flags |= flags;
        } else {
                ifr.ifr_flags &= ~flags;
        }
        
        if (ioctl(sock, SIOCSIFFLAGS, &ifr) < 0) {
                DLOG_ERR("Could not set interface %s flags\n", 
                         wlan_status.ifname); 
                return -1;
        }
        
        DLOG_DEBUG("%s is %s", wlan_status.ifname, dir == SET ? "UP":"DOWN");

        return 0;
}

static gboolean set_tx_power(guint power, int sock) 
{
        struct iwreq req;
        init_iwreq(&req);
        
        req.u.txpower.fixed = 1;
        req.u.txpower.disabled = 0;
        req.u.txpower.flags = IW_TXPOW_DBM;

        if (power == WLANCOND_TX_POWER10) {
                req.u.txpower.value = WLANCOND_TX_POWER10DBM;
        } else if (power == WLANCOND_TX_POWER100) {
                req.u.txpower.value = WLANCOND_TX_POWER100DBM;
        } else {
                return FALSE;
        }
        
        if (ioctl(sock, SIOCSIWTXPOW, &req) < 0) {
                DLOG_ERR("set power failed\n");
                return FALSE;
        }
        return TRUE;
        
}
static int update_algorithms(guint32 encryption) 
{
        wlan_status.group_cipher = 0;
        wlan_status.pairwise_cipher = 0;

        // Open
        if ((encryption & WLANCOND_ENCRYPT_METHOD_MASK) == WLANCOND_OPEN) {
                DLOG_DEBUG("Open mode");    
                wlan_status.pairwise_cipher = CIPHER_SUITE_NONE;
                return 0;
        }
        
        // WEP
        if ((encryption & WLANCOND_ENCRYPT_METHOD_MASK) == WLANCOND_WEP) {
                DLOG_DEBUG("WEP enabled");
                wlan_status.pairwise_cipher = CIPHER_SUITE_WEP40;
                return 0;
        } 
        
        // WPA modes
        if ((encryption & WLANCOND_ENCRYPT_ALG_MASK) 
                   == WLANCOND_WPA_TKIP) {
                DLOG_DEBUG("TKIP Selected for unicast");
                wlan_status.pairwise_cipher = CIPHER_SUITE_TKIP;
        } else if ((encryption & WLANCOND_ENCRYPT_ALG_MASK) == 
                   WLANCOND_WPA_AES) {
                DLOG_DEBUG("AES selected for unicast");
                wlan_status.pairwise_cipher = CIPHER_SUITE_CCMP;
                
        } else {
                DLOG_ERR("Not supported encryption %08x", encryption);
                return -1;
        }
        
        if ((encryption & WLANCOND_ENCRYPT_GROUP_ALG_MASK) == 
            WLANCOND_WPA_TKIP_GROUP) {
                DLOG_DEBUG("TKIP Selected for multicast");
                wlan_status.group_cipher = CIPHER_SUITE_TKIP;
        } else if ((encryption & WLANCOND_ENCRYPT_GROUP_ALG_MASK) == 
                   (unsigned int)WLANCOND_WPA_AES_GROUP) {
                DLOG_DEBUG("AES Selected for multicast");
                wlan_status.group_cipher = CIPHER_SUITE_CCMP;
        } else {
                DLOG_ERR("Not supported encryption %08x", encryption);
                return -1;
        }       

        return 0;
}
void clean_roam_cache(void) 
{
        clean_scan_results(&wlan_status.roam_cache);
}
void clear_wpa_mode(void) 
{
        g_free(wlan_status.wpa_ie.ie);
        wlan_status.wpa_ie.ie_len = 0;
        wlan_status.wpa_ie.ie = NULL;

        wlan_status.pairwise_cipher = CIPHER_SUITE_NONE;
        wlan_status.group_cipher = CIPHER_SUITE_NONE;
        
        //set_encryption_method(wlan_status.pairwise_cipher, &wlan_status);
        clear_wpa_keys(NULL);
        /* Clean PMK cache */
        g_slist_foreach(wlan_status.pmk_cache, (GFunc)g_free, NULL);
        g_slist_free(wlan_status.pmk_cache);
        wlan_status.pmk_cache = NULL;
}
gboolean get_wpa_mode(void)
{
        if (wlan_status.pairwise_cipher & CIPHER_SUITE_TKIP ||
            wlan_status.pairwise_cipher & CIPHER_SUITE_CCMP) {
                return TRUE;
        }
        return FALSE;
}

static gint compare_pmk_entry(gconstpointer a, gconstpointer b) 
{
        const struct pmksa_cache_t *pmk_cache = a;
        
        return memcmp(pmk_cache->mac, b, ETH_ALEN);
}

static void add_to_pmksa_cache(unsigned char* pmkid, unsigned char* mac)
{
        guint i = 0;
        GSList *list;
        gboolean entry_found = FALSE;
        
        for (list = wlan_status.pmk_cache; list != NULL && entry_found == FALSE; list = list->next) {
                struct pmksa_cache_t *pmk_cache = list->data;
                /* First find if we have already the cache entry */
                if (memcmp(pmk_cache->mac, mac, ETH_ALEN) == 0) {
                        DLOG_DEBUG("Found old entry: %i", i);
                        /* Remove the old entry */
                        wlan_status.pmk_cache = g_slist_remove(wlan_status.pmk_cache, pmk_cache);
                        g_free(pmk_cache);
                        
                        entry_found = TRUE;
                } else {
                        i++;
                }
        }
        
        if (i == PMK_CACHE_SIZE) {
                DLOG_DEBUG("Cache full, remove oldest");
                GSList *last_entry = g_slist_last(wlan_status.pmk_cache);
                wlan_status.pmk_cache = g_slist_remove(wlan_status.pmk_cache, last_entry->data);
                g_free(last_entry->data);
        }
        print_mac("Adding new entry:", mac);
        
        struct pmksa_cache_t *new_entry = g_malloc(sizeof(*new_entry));
        memcpy(new_entry->mac, mac, ETH_ALEN);
        memcpy(new_entry->pmkid, pmkid, IW_PMKID_LEN);
        
        wlan_status.pmk_cache = g_slist_prepend(wlan_status.pmk_cache, new_entry);
        return;
}
unsigned char* find_pmkid_from_pmk_cache(unsigned char* mac) 
{
        GSList *list;

        list = g_slist_find_custom(wlan_status.pmk_cache, mac, &compare_pmk_entry);
        if (list != NULL) {
                struct pmksa_cache_t *pmk_cache = list->data;
                print_mac("Found PMKSA entry for:", mac);
                return pmk_cache->pmkid;
        }
        return NULL;
}

int scan(gchar *ssid, int ssid_len, gboolean add_timer)
{
        struct iwreq req;
        struct iw_scan_req scan_req;
        
        if (get_scan_state() == SCAN_ACTIVE)
                return 0;
        
        set_scan_state(SCAN_ACTIVE);

        init_iwreq(&req);
        
        memset(&scan_req, 0, sizeof(scan_req));
        
        if (ssid_len > 1 && ssid != NULL) {
                //DLOG_DEBUG("Active scan for: %s (len=%d)", ssid, ssid_len -1);
                scan_req.essid_len = ssid_len -1;
                scan_req.bssid.sa_family = ARPHRD_ETHER;
                memset(scan_req.bssid.sa_data, 0xff, ETH_ALEN);
                memcpy(scan_req.essid, ssid, ssid_len -1);
                req.u.data.pointer = (caddr_t) &scan_req;
                req.u.data.length = sizeof(scan_req);
                req.u.data.flags = IW_SCAN_THIS_ESSID;
        }
        
        if (ioctl(socket_open(), SIOCSIWSCAN, &req) < 0) {
                DLOG_ERR("Scan failed");
                return -1;
        }
        
        if (add_timer == TRUE) {
                wlan_status.scan_id = g_timeout_add_seconds(
                        WLANCOND_SCAN_TIMEOUT,
                        wlan_scan_cb,
                        NULL);
        }

        DLOG_DEBUG("Scan issued");
        
        return 0;       
}
static int set_freq(int channel)
{
        struct iwreq req;
        
        DLOG_DEBUG("Setting channel: %d", channel);
        
        init_iwreq(&req);
        
        req.u.freq.m = channel;
        
        if (ioctl(socket_open(), SIOCSIWFREQ, &req) < 0) {
                DLOG_ERR("Freq failed");
                return -1;
        }
        
        return 0;
}
static void init_conn_params(struct connect_params_t *conn_params) 
{
        memset(conn_params, 0, sizeof(*conn_params));
}
void clear_wpa_keys(unsigned char* bssid)
{
        struct iwreq req;
        struct iw_encode_ext ext;
        int sock;
        guint i;
        
        init_iwreq(&req);
        
        sock = socket_open();
        
        for (i=0;i<4;i++) {
                
                memset(&ext, 0, sizeof(ext));
                
                req.u.encoding.flags = i + 1;
                req.u.encoding.flags |= IW_ENCODE_DISABLED;
                req.u.encoding.pointer = (caddr_t) &ext;
                req.u.encoding.length = sizeof(ext);
                
                ext.ext_flags |= IW_ENCODE_EXT_GROUP_KEY;
                ext.addr.sa_family = ARPHRD_ETHER;
                
                memset(ext.addr.sa_data, 0xff, ETH_ALEN);
                ext.alg = IW_ENCODE_ALG_NONE;
                
                if (ioctl(sock, SIOCSIWENCODEEXT, &req) < 0) {
                        DLOG_ERR("Key clearing failed");
                }
        }
        if (bssid != NULL) {
                
                memset(&ext, 0, sizeof(ext));
                
                req.u.encoding.flags = 1; //Revisit
                req.u.encoding.flags |= IW_ENCODE_DISABLED;
                req.u.encoding.pointer = (caddr_t) &ext;
                req.u.encoding.length = sizeof(ext);
                
                ext.addr.sa_family = ARPHRD_ETHER;
                
                memcpy(ext.addr.sa_data, bssid, ETH_ALEN);
                ext.alg = IW_ENCODE_ALG_NONE;
                
                if (ioctl(sock, SIOCSIWENCODEEXT, &req) < 0) {
                        DLOG_ERR("Key clearing failed");
                }
        }
        
        return;
}
static int check_connect_arguments(struct connect_params_t *conn, char* ssid,
                                   unsigned char** key) 
{
        guint i;
        
        if (conn->flags & WLANCOND_DISABLE_POWERSAVE) {
                DLOG_DEBUG("Powersave disabled");
                powersave = WLANCOND_POWER_ON;
        } else if (conn->flags & WLANCOND_MINIMUM_POWERSAVE) {
                DLOG_DEBUG("Powersave minimum");
                powersave = WLANCOND_LONG_CAM;
        } else if (conn->flags & WLANCOND_MAXIMUM_POWERSAVE) {
                DLOG_DEBUG("Powersave maximum");
                powersave = WLANCOND_SHORT_CAM;
        } else {
                powersave = WLANCOND_SHORT_CAM;
        }
        
        if (conn->power_level != WLANCOND_TX_POWER10 &&
            conn->power_level != WLANCOND_TX_POWER100) {
                DLOG_ERR("Invalid power level");
                return -1;
        }
        
        switch (conn->mode) {
            case WLANCOND_ADHOC:
            case WLANCOND_INFRA:
                    break;
            default:
                    DLOG_ERR("Operating mode undefined\n");
                    return -1;
        }
        /* Encryption settings */
        guint32 wpa2_mode = conn->encryption & WLANCOND_ENCRYPT_WPA2_MASK;
        
        DLOG_DEBUG("Encryption setting: %08x", conn->encryption);
        
        switch (conn->encryption & WLANCOND_ENCRYPT_METHOD_MASK) {
            case WLANCOND_OPEN:
                    break;
            case WLANCOND_WEP:
                    break;
            case WLANCOND_WPA_PSK:
                    DLOG_DEBUG("%s PSK selected", wpa2_mode!=0?"WPA2":"WPA"); 
                    if (wpa2_mode != 0)
                            conn->authentication_type = EAP_AUTH_TYPE_WPA2_PSK;
                    else
                            conn->authentication_type = EAP_AUTH_TYPE_WPA_PSK;
                    break;
            case WLANCOND_WPA_EAP:
                    DLOG_DEBUG("%s EAP selected", wpa2_mode!=0?"WPA2":"WPA");
                    if (wpa2_mode != 0)
                            conn->authentication_type = EAP_AUTH_TYPE_WPA2_EAP;
                    else
                            conn->authentication_type = EAP_AUTH_TYPE_WPA_EAP;
                    break;
            default:
                    DLOG_DEBUG("Unsupported encryption mode");
                    return -1;
        }
        if ((conn->encryption & WLANCOND_WPS_MASK) != 0) {
                DLOG_DEBUG("WPS selected");
                conn->authentication_type = EAP_AUTH_TYPE_WFA_SC;
        }
        
        if (!ssid || conn->ssid_len == 0 || 
            conn->ssid_len > WLANCOND_MAX_SSID_SIZE + 1) {
                DLOG_DEBUG("Invalid SSID");
                return -1;
        }
        for (i=0;i<4;i++) {
                if (conn->key_len[i] != 0) {
                        DLOG_DEBUG("Found key %d", i);
                        memcpy(&conn->key[i][0], key[i], conn->key_len[i]);
                }
        }
        return 0;
}
        
static DBusHandlerResult settings_and_connect_request(
        DBusMessage    *message,
        DBusConnection *connection) {
        
        DBusMessage *reply = NULL;
        DBusError derror;
        struct connect_params_t *conn;   
        char *ssid;
        unsigned char* key[4];
        dbus_int32_t old_mode;

        dbus_error_init(&derror);

        if (in_flight_mode()) {
                reply = new_dbus_error(message, WLANCOND_ERROR_WLAN_DISABLED);
                send_and_unref(connection, reply);
                return DBUS_HANDLER_RESULT_HANDLED;
        }

        remove_wlan_if_timer();
        
        conn = &wlan_status.conn;
        /* Save previous mode */
        old_mode = conn->mode;
        init_conn_params(conn);

        if (dbus_message_get_args(
                    message, NULL,        
                    DBUS_TYPE_INT32, &conn->power_level,
                    DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &ssid, &conn->ssid_len,
                    DBUS_TYPE_INT32, &conn->mode,
                    DBUS_TYPE_INT32, &conn->encryption,
                    DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &key[0], &conn->key_len[0],
                    DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &key[1], &conn->key_len[1],
                    DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &key[2], &conn->key_len[2],
                    DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &key[3], &conn->key_len[3],
                    DBUS_TYPE_INT32, &conn->default_key,
                    DBUS_TYPE_UINT32, &conn->adhoc_channel,
                    DBUS_TYPE_UINT32, &conn->flags,
                    DBUS_TYPE_INVALID) == FALSE) 
        {
                /* Try without flags */
                if (dbus_message_get_args(
                            message, &derror,        
                            DBUS_TYPE_INT32, &conn->power_level,
                            DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &ssid, 
                            &conn->ssid_len,
                            DBUS_TYPE_INT32, &conn->mode,
                            DBUS_TYPE_INT32, &conn->encryption,
                            DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, 
                            &key[0], &conn->key_len[0],
                            DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, 
                            &key[1], &conn->key_len[1],
                            DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, 
                            &key[2], &conn->key_len[2],
                            DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, 
                            &key[3], &conn->key_len[3],
                            DBUS_TYPE_INT32, &conn->default_key,
                            DBUS_TYPE_UINT32, &conn->adhoc_channel,
                            DBUS_TYPE_INVALID) == FALSE) {

                        DLOG_ERR("Failed to parse setting_and_connect: %s",
                                 derror.message);
                        dbus_error_free(&derror);
                        goto param_err;
                }
        }

        if (check_connect_arguments(conn, ssid, key) < 0)
                goto param_err;

        set_power_state(WLANCOND_POWER_ON, socket_open());

        /* If we change mode, do it when interface is down.
           Also put interface down in WPS mode to clear old
           scan results.
        */
        if (old_mode != conn->mode || 
            conn->encryption & WLANCOND_WPS_PUSH_BUTTON) {
                set_wlan_state(WLAN_NOT_INITIALIZED, NO_SIGNAL, FORCE_YES);
        }

        /* Mode */
        if (set_mode(conn->mode) < 0) {
                goto param_err;
        }
        
        if (init_if(socket_open()) < 0) {
                reply = new_dbus_error(message, WLANCOND_ERROR_INIT_FAILED);
                goto param_err;
        }

        if (set_tx_power(conn->power_level, socket_open()) != TRUE) {
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                goto param_err;
        }
        
        memcpy(conn->ssid, ssid, conn->ssid_len);

        set_scan_state(SCAN_NOT_ACTIVE);        
        set_wlan_state(WLAN_INITIALIZED_FOR_CONNECTION, NO_SIGNAL, FORCE_NO);
        
        /* Make a broadcast scan to catch all WPS PBC registrars */
        if (conn->encryption & WLANCOND_WPS_PUSH_BUTTON) {
                DLOG_DEBUG("Broadcast scan for WPS");
                if (scan(NULL, 0, TRUE) < 0) {
                        goto param_err;
                }
        } else {
                /* Try if our own cache has results */
                if (find_connection_and_associate(wlan_status.roam_cache, 
                                                  FALSE, FALSE) != 0) {
                        /* Try if mac80211 has cached results */
                        DLOG_DEBUG("Checking mac80211 cache...");
                        
                        GSList *scan_results = NULL;                
                        scan_results_ioctl(0, &scan_results);
                        
                        if (find_connection_and_associate(scan_results, TRUE, 
                                                          FALSE) != 0) {
                                clean_scan_results(&scan_results);
                                if (scan(conn->ssid, conn->ssid_len, TRUE) < 0) {
                                        goto param_err;
                                }
                        } else {
                                clean_scan_results(&scan_results);
                        }
                }
        }
        
        g_free(connect_name_cache);
        connect_name_cache = g_strdup(dbus_message_get_sender(message));
        
        reply = new_dbus_method_return(message);
        
        gchar* ifname = wlan_status.ifname;

        append_dbus_args(reply,
                         DBUS_TYPE_STRING, &ifname,
                         DBUS_TYPE_INVALID);        
        
        send_and_unref(connection, reply);
        
        return DBUS_HANDLER_RESULT_HANDLED;
        
  param_err:
        if (reply == NULL) {
                DLOG_DEBUG("Parameter error in settings_and_connect\n");
                reply = new_dbus_error(message, DBUS_ERROR_INVALID_ARGS);
        }
        send_and_unref(connection, reply);
        return DBUS_HANDLER_RESULT_HANDLED;
}
int associate(struct scan_results_t *scan_results) 
{
        struct connect_params_t *conn = &wlan_status.conn;

        DLOG_DEBUG("Starting associate...");

        if (memcmp(conn->bssid, "\0\0\0\0\0\0", ETH_ALEN)) {
                clear_wpa_keys(conn->bssid);
        }

        if (update_algorithms(conn->encryption) < 0) {
                return -1;
        }
        
        /* WEP keys */
        if ((conn->encryption & WLANCOND_ENCRYPT_METHOD_MASK) == WLANCOND_WEP){
                if (set_wep_keys(conn) < 0) {
                        return -1;
                }
        }
        
        memcpy(conn->bssid, scan_results->bssid, ETH_ALEN);

        if (set_encryption_method(conn->encryption, &wlan_status) == FALSE) {
                return -1;
        }

        if (get_wpa_mode() == TRUE || 
            conn->authentication_type == EAP_AUTH_TYPE_WFA_SC) {

                // Initialize authentication SW if mode is WPA or WPS
                if (wpa_ie_push(scan_results->bssid, 
                                scan_results->wpa_ie, 
                                scan_results->wpa_ie_len, 
                                scan_results->ssid, 
                                scan_results->ssid_len -1,
                                conn->authentication_type) < 0)
                        return -1;
        }

        /* Channel */
        /* Ad-hoc channel */
        if (conn->adhoc_channel != 0 && (conn->mode & WLANCOND_ADHOC)) {
                if (conn->adhoc_channel < WLANCOND_MIN_WLAN_CHANNEL ||
                    conn->adhoc_channel > WLANCOND_MAX_WLAN_CHANNEL) {
                        DLOG_ERR("Invalid ad-hoc channel: %d", 
                                 conn->adhoc_channel);
                        return -1;
                }
                
                scan_results->channel = conn->adhoc_channel;
        }
        
        set_freq(scan_results->channel);

        /* Set BSSID if known and no Adhoc */
        if (conn->mode != WLANCOND_ADHOC && 
            memcmp(scan_results->bssid, "\0\0\0\0\0\0", ETH_ALEN)) {
                if (set_bssid(scan_results->bssid) < 0) {
                        return -1;
                }
        }
        /* ESSID */
        if (set_essid(conn->ssid, conn->ssid_len) < 0) {
                return -1;
        }

        /* Set association timeout timer */
        wlan_connect_timer_id = g_timeout_add_seconds(
                WLANCOND_CONNECT_TIMEOUT, 
                wlan_connect_timer_cb, NULL);
        
        return 0;
}

static DBusHandlerResult scan_request(DBusMessage    *message,
                                      DBusConnection *connection) {
        
        DBusMessage *reply = NULL;
        DBusMessageIter iter, array_iter;
        char *ssid;
        const char* sender;
        dbus_int32_t power_level;
        dbus_int32_t flags;
        gint previous_state = 0;
        
        if (in_flight_mode()) {
                reply = new_dbus_error(message, WLANCOND_ERROR_WLAN_DISABLED);
                send_and_unref(connection, reply);
                return DBUS_HANDLER_RESULT_HANDLED;
        }

        sender = dbus_message_get_sender(message);
        if (sender == NULL) {
                goto param_err;
        }

        DLOG_DEBUG("Got scan request from %s", sender);
        
        /* Do not scan if we are scanning already or if we are associating */
        if (get_scan_state() == SCAN_ACTIVE || wlan_connect_timer_id != 0) {
                reply = new_dbus_error(message, WLANCOND_ERROR_ALREADY_ACTIVE);
                send_and_unref(connection, reply);
                return DBUS_HANDLER_RESULT_HANDLED;
        }
        
        if ((previous_state = init_if(socket_open())) < 0) {
                reply = new_dbus_error(message, WLANCOND_ERROR_INIT_FAILED);
                goto param_err;
        }
        if (previous_state == WLAN_NOT_INITIALIZED) {
                set_wlan_state(WLAN_INITIALIZED_FOR_SCAN, NO_SIGNAL, FORCE_NO);
        }
        
        dbus_message_iter_init(message, &iter);
        
        if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_INT32)
                goto param_err;
        dbus_message_iter_get_basic(&iter, &power_level);

        dbus_message_iter_next(&iter);
        
        if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_ARRAY ||
            dbus_message_iter_get_element_type(&iter) != DBUS_TYPE_BYTE)
                goto param_err;
        dbus_message_iter_recurse(&iter, &array_iter);
        dbus_message_iter_get_fixed_array(&array_iter, &ssid, 
                                          &wlan_status.scan_ssid_len);

        if (wlan_status.scan_ssid_len > WLANCOND_MAX_SSID_SIZE+1)
                goto param_err;
        
        dbus_message_iter_next(&iter);
        
        power_down_after_scan = FALSE;
        
        if (dbus_message_iter_get_arg_type(&iter) == DBUS_TYPE_UINT32) {
                dbus_message_iter_get_basic(&iter, &flags);
                DLOG_DEBUG("Found flags: %08x", flags);
                
                if (flags & WLANCOND_NO_DELAYED_SHUTDOWN)
                        power_down_after_scan = TRUE;
        }
        
        if (power_level != WLANCOND_TX_POWER10 &&
            power_level != WLANCOND_TX_POWER100) {
                DLOG_ERR("Invalid power level");
                goto param_err;
        }
        
        if (set_tx_power(power_level, socket_open()) != TRUE) {
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                goto param_err;
        }
        
        memset(wlan_status.scan_ssid, 0, sizeof(wlan_status.scan_ssid));

        if (ssid != NULL && wlan_status.scan_ssid_len > 1) {
                memcpy(wlan_status.scan_ssid, ssid, wlan_status.scan_ssid_len);
        }
        
        if (scan(wlan_status.scan_ssid, wlan_status.scan_ssid_len, TRUE) < 0) { 
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                goto param_err;
        }

        g_free(scan_name_cache);
        scan_name_cache = g_strdup(sender);
        
        reply = new_dbus_method_return(message);
        send_and_unref(connection, reply);
        
        return DBUS_HANDLER_RESULT_HANDLED;
        
  param_err:
        if (get_wlan_state() == WLAN_INITIALIZED_FOR_SCAN) {
                set_wlan_state(WLAN_NOT_INITIALIZED, NO_SIGNAL, FORCE_YES);
        }
        if (reply == NULL) {
                DLOG_DEBUG("Parameter error in scan request");
                reply = new_dbus_error(message, DBUS_ERROR_INVALID_ARGS);
        }
        send_and_unref(connection, reply);
        return DBUS_HANDLER_RESULT_HANDLED;
}
static int network_compare(gconstpointer a, gconstpointer b)
{
        const struct scan_results_t *results_a = a;
        const struct scan_results_t *results_b = b;
        
        if (wlan_status.scan_ssid_len > 1) {
                
                //DLOG_DEBUG("Scan ssid = %s", scan_ssid);
                
                gint a_eq = strncmp(wlan_status.scan_ssid, results_a->ssid, WLANCOND_MAX_SSID_SIZE);
                gint b_eq = strncmp(wlan_status.scan_ssid, results_b->ssid, WLANCOND_MAX_SSID_SIZE);
                // Check if either network match the scan SSID
                if (!a_eq && !b_eq) {
                        //DLOG_DEBUG("Both (%s, %s) match scan SSID", results_a->ssid, results_b->ssid);
                        return 0;
                }
                
                if (!a_eq && b_eq) {
                        //DLOG_DEBUG("%s is better than %s", results_a->ssid, results_b->ssid);
                        return -1;
                }
                
                if (a_eq && !b_eq) {
                        
                        //DLOG_DEBUG("%s is better than %s", results_b->ssid, results_a->ssid);
                        return 1;
                }
                
        }
        //DLOG_DEBUG("No scan ssid, returning just RSSI values");

        return (results_a->rssi > results_b->rssi) ? -1 : (results_a->rssi < results_b->rssi) ? 1 : 0;
}

static gint compare_scan_entry(gconstpointer a, gconstpointer b) 
{
        const struct scan_results_t *scan_entry = a;
        return memcmp(scan_entry->bssid, b, ETH_ALEN);
}
static void add_to_roam_cache(struct scan_results_t *scan_results) 
{
        GSList *list;
        
        list = g_slist_find_custom(wlan_status.roam_cache, scan_results->bssid,
                                   &compare_scan_entry);
        
        if (list != NULL) {
                struct scan_results_t *roam_cache_entry = list->data;
                print_mac("Found old entry for:", scan_results->bssid);
                
                /* Remove the old entry */
                wlan_status.roam_cache = g_slist_remove(
                        wlan_status.roam_cache, roam_cache_entry);
                
                clean_scan_results_item(roam_cache_entry, NULL);
        }

        print_mac("New AP to roam cache:", scan_results->bssid);
        
        struct scan_results_t *results_to_list = g_slice_dup(struct scan_results_t, scan_results);
        results_to_list->wpa_ie = g_memdup(scan_results->wpa_ie, 
                                           scan_results->wpa_ie_len);
        wlan_status.roam_cache = g_slist_prepend(wlan_status.roam_cache, 
                                                 results_to_list);
        
        return;
}
gboolean remove_from_roam_cache(unsigned char *bssid) 
{
        GSList *list;

        list = g_slist_find_custom(wlan_status.roam_cache, bssid,
                                   &compare_scan_entry);
        
        if (list != NULL) {
                struct scan_results_t *roam_cache_entry = list->data;
                print_mac("Found entry to be removed:", bssid);
                
                wlan_status.roam_cache = g_slist_remove(
                        wlan_status.roam_cache, roam_cache_entry);
                
                clean_scan_results_item(roam_cache_entry, NULL);
                
                return TRUE;
        }
        
        return FALSE;
}

gboolean decrease_signal_in_roam_cache(unsigned char *bssid) 
{
        GSList *list;

        list = g_slist_find_custom(wlan_status.roam_cache, bssid,
                                   &compare_scan_entry);
        
        if (list != NULL) {
                struct scan_results_t *roam_cache_entry = list->data;
                print_mac("Found entry to be decreased:", bssid);
                
                roam_cache_entry->rssi -= WLANCOND_RSSI_PENALTY;
                
                return TRUE;
        }
        
        return FALSE;
}
static int check_group_cipher(guint32 c1, guint32 c2) 
{
        guint32 m1 = (c1 & WLANCOND_ENCRYPT_GROUP_ALG_MASK);
        guint32 m2 = (c2 & WLANCOND_ENCRYPT_GROUP_ALG_MASK);

        if (m1 == m2)
                return 1;
        if (m2 == WLANCOND_WPA_TKIP_GROUP && (m1 & WLANCOND_WPA_TKIP_GROUP))
                return 1;
        if (m2 == (unsigned int)WLANCOND_WPA_AES_GROUP && 
            (m1 & WLANCOND_WPA_AES_GROUP))
                return 1;
        
        DLOG_DEBUG("Group ciphers don't match");
        
        return -1;
}
static int check_ciphers(guint32 c1, guint32 c2) 
{
        guint32 u1 = (c1 & WLANCOND_ENCRYPT_ALG_MASK);
        guint32 u2 = (c2 & WLANCOND_ENCRYPT_ALG_MASK);
        
        if (check_group_cipher(c1, c2) < 0)
                return -1;
        
        if (u1 == u2)
                return 1;
        if (u2 == WLANCOND_WPA_TKIP && (u1 & WLANCOND_WPA_TKIP))
                return 1;
        if (u2 == WLANCOND_WPA_AES && (u1 & WLANCOND_WPA_AES))
                return 1;
        
        DLOG_DEBUG("Unicast ciphers don't match");
        
        return -1;
}
static gboolean wlan_roam_scan_cb(void* data) 
{
        wlan_status.roam_scan_id = 0;
        
        if (wlan_status.signal == WLANCOND_LOW && 
            get_wlan_state() == WLAN_CONNECTED) {
                
                DLOG_DEBUG("Roam scan timeout, initiating new scan");
                
                // Ignore return value
                scan(wlan_status.conn.ssid, wlan_status.conn.ssid_len, FALSE);
        }
        
        return FALSE;
}

void schedule_scan(guint seconds) {

        /* Remove old timer */
        remove_roam_scan_timer();
        wlan_status.roam_scan_id = g_timeout_add_seconds(
                seconds,
                wlan_roam_scan_cb,
                NULL);
}

static void reschedule_scan(void) 
{
        /* 
           If we are active use shortest scan interval, otherwise
           use exponential backoff
        */
        if (get_inactivity_status() == TRUE) {
                wlan_status.roam_scan = WLANCOND_MIN_ROAM_SCAN_INTERVAL;
        } else {
                if (wlan_status.roam_scan <= WLANCOND_MIN_ROAM_SCAN_INTERVAL) {
                        wlan_status.roam_scan = WLANCOND_MIN_ROAM_SCAN_INTERVAL;
                } else if (wlan_status.roam_scan >= WLANCOND_MAX_ROAM_SCAN_INTERVAL) {
                        wlan_status.roam_scan = WLANCOND_MAX_ROAM_SCAN_INTERVAL;
                } else {
                        wlan_status.roam_scan = wlan_status.roam_scan * 2;
                }
        }
        
        schedule_scan(wlan_status.roam_scan);
}

static gboolean check_capabilities(struct scan_results_t *scan_results,
                                   struct connect_params_t *conn) 
{
        // Check mode
        if ((scan_results->cap_bits & WLANCOND_MODE_MASK) != (guint32)conn->mode)
                return FALSE;
        if ((scan_results->cap_bits & WLANCOND_ENCRYPT_METHOD_MASK) != (guint32)(conn->encryption & WLANCOND_ENCRYPT_METHOD_MASK))
                return FALSE;
        if ((scan_results->cap_bits & WLANCOND_ENCRYPT_WPA2_MASK) != (guint32)(conn->encryption & WLANCOND_ENCRYPT_WPA2_MASK))
                return FALSE;
        if (check_ciphers(scan_results->cap_bits, conn->encryption) < 0)
                return FALSE;
        
        return TRUE;
}

struct scan_results_t* find_connection(
        GSList* ap_list, struct connect_params_t *conn, 
        gboolean update_roam_cache)
{
        GSList *list;
        struct scan_results_t *best_connection = NULL;
        gint current_rssi = 0;

        /* If update roam cache, clean it first */
        if (update_roam_cache == TRUE)
                clean_roam_cache();
        
        for (list = ap_list; list != NULL; list = list->next) {
                struct scan_results_t *scan_results = list->data;
                if (memcmp(scan_results->ssid, conn->ssid, scan_results->ssid_len) == 0) {
                        print_mac("Found AP:", scan_results->bssid);
                        
                        /* Find the current AP so that we know it's RSSI */
                        if (memcmp(scan_results->bssid, wlan_status.conn.bssid,
                                   ETH_ALEN) == 0) {
                                current_rssi = scan_results->rssi;
                                DLOG_DEBUG("Current AP: %d", current_rssi);
                        }
                        
                        if (check_capabilities(scan_results, conn) == 
                            FALSE)
                                continue;

                        if (is_ap_in_black_list(scan_results->bssid) == TRUE) {
                                DLOG_DEBUG("AP is in black list, discarded");
                                continue;
                        }
                        
                        /* At this point we know the connection is good,
                           add to the roam cache
                        */
                        if (update_roam_cache == TRUE) {
                                add_to_roam_cache(scan_results);
                        }
                        
                        if (best_connection == NULL || 
                            best_connection->rssi < scan_results->rssi) {
                                DLOG_DEBUG("Best connection: %d (old %d)", 
                                           scan_results->rssi, 
                                           best_connection == NULL ? 
                                           WLANCOND_MINIMUM_SIGNAL:
                                           best_connection->rssi);
                                best_connection = scan_results;
                        }
                }
        }
        
        /* Did not find any connection */
        if (best_connection == NULL)
                return NULL;
        
        /* Check if we are already connected but the best connection is not 
           worth changing
        */
        if (current_rssi != 0) {
                if (best_connection->rssi < current_rssi + 
                    WLANCOND_ROAM_THRESHOLD) {
                        DLOG_DEBUG("Best connection not good enough");
                        return NULL;
                }
        }

        /* Check if the signal level is good enough for connection */
        if (best_connection->rssi < WLANCOND_MINIMUM_SIGNAL)
                return NULL;
        
        return best_connection;
}
static dbus_uint32_t find_adhoc_channel(GSList *ap_list) {
        
        dbus_uint32_t used_channel_list = 0;
        dbus_uint32_t selected_channel = 0;
        GSList* list;
        guint32 i;
        
        /* Create a map of free channels */
        for (list = ap_list; list != NULL; list = list->next) {
                struct scan_results_t *scan_results = list->data;
                used_channel_list |= 1 << scan_results->channel;
        }
        
        for (i = 1; i <= 11; i++) {
                if (!(used_channel_list & (1 << i))) {
                        selected_channel = i;
                        break;
                }
        }
        
        if (selected_channel == 0) {
                /* No free channel found, choose one randomly from
                 * channels 1 - 11. */
                selected_channel = g_random_int_range(1, 12);
        }

        //DLOG_DEBUG("Selected adhoc channel: %d", selected_channel);

        return selected_channel;
}
int find_connection_and_associate(GSList *scan_results, 
                                  gboolean update_roam_cache,
                                  gboolean create_new_adhoc) 
{
        struct scan_results_t adhoc;
        struct connect_params_t *conn = &wlan_status.conn;
        guint wps_pbc_registrars = 0;
        GSList* list;

        /* Check for too many registrars in the WPS PBC 
           method */
        if (conn->encryption & WLANCOND_WPS_PUSH_BUTTON) {
                for (list = scan_results; list != NULL; list = list->next) {
                        struct scan_results_t *scan_results = list->data;
                        
                        if (scan_results->cap_bits & WLANCOND_WPS_PUSH_BUTTON 
                            && 
                            scan_results->cap_bits & WLANCOND_WPS_CONFIGURED) {
                                if (++wps_pbc_registrars > 1) {
                                        DLOG_ERR("Too many WPS PBC registrars");
                                        return ETOOMANYREGISTRARS;
                                }
                        }
                }
        }

        struct scan_results_t *connection = find_connection(
                scan_results, &wlan_status.conn, update_roam_cache);
        
        if (connection == NULL && conn->mode == WLANCOND_ADHOC &&
            create_new_adhoc == TRUE) {
                DLOG_DEBUG("No existing adhoc connection");
                memset(&adhoc, 0, sizeof(adhoc));
                connection = &adhoc;
                memcpy(connection->ssid, conn->ssid, conn->ssid_len);
                connection->channel = find_adhoc_channel(scan_results);
        }       
        
        if (connection) {
                int ret = associate(connection);
                if (ret < 0)
                        memset(wlan_status.conn.bssid, 0, ETH_ALEN);
                return ret;
        }
        return -1;
}
static void registrar_error_signal(void)
{
        DBusMessage *registrar_error;
        
        registrar_error = new_dbus_signal(
                WLANCOND_SIG_PATH,        
                WLANCOND_SIG_INTERFACE,
                WLANCOND_REGISTRAR_ERROR_SIG,
                NULL);
        
        send_and_unref(get_dbus_connection(), registrar_error);
}
static gboolean rescan(void* data) 
{
        if (get_wlan_state() == WLAN_INITIALIZED_FOR_CONNECTION) {
                // Ignore return value
                scan(wlan_status.conn.ssid, wlan_status.conn.ssid_len, FALSE);
        }
        return FALSE;
}
static void connect_from_scan_results(GSList *scan_results) 
{
        int status = find_connection_and_associate(scan_results, TRUE, TRUE);
        
        clean_scan_results(&scan_results);
        
        if (status == 0)
                return;
        
        DLOG_DEBUG("Could not find suitable network");
        
        if (get_wlan_state() == WLAN_INITIALIZED_FOR_CONNECTION) {
                /* Try rescanning if retries left */
                if (++wlan_status.retry_count <= WLANCOND_MAX_SCAN_TRIES) {
                        DLOG_DEBUG("Rescanning");
                        g_timeout_add_seconds(
                                WLANCOND_RESCAN_DELAY,
                                rescan,
                                NULL); 
                } else {
                        /* In WPS too many registrars case we send
                           different signal
                        */
                        if (status == ETOOMANYREGISTRARS) {
                                set_wlan_state(WLAN_NOT_INITIALIZED, 
                                               NO_SIGNAL, FORCE_YES);
                                registrar_error_signal();
                        } else {
                                set_wlan_state(WLAN_NOT_INITIALIZED, 
                                               DISCONNECTED_SIGNAL, FORCE_YES);
                        }
                }
                return;
        }
        
        /* We are connected but would prefer better connection */
        reschedule_scan();
}
int scan_results_ioctl(int ifindex, GSList** scan_results_save) 
{
        struct iwreq req;
        char *buffer;
        unsigned int buflen = IW_SCAN_MAX_DATA*2;
        int sock;
        unsigned int counter = 3;

        //DLOG_DEBUG("Scan results ioctl");

        init_iwreq(&req);

        sock = socket_open();
        
        buffer = g_malloc(buflen);
        
  try_again:
        
        /* Read the results */
        req.u.data.pointer = buffer;
        req.u.data.flags = 0;
        req.u.data.length = buflen;
        
        if (ioctl(sock, SIOCGIWSCAN, &req) < 0) {
                /* Check if we got too many results for 
                   our buffer*/
                if (errno == E2BIG && buflen != G_MAXUINT16) {
                        DLOG_DEBUG("Too much data for buffer length %d "
                                   "needed %d\n", buflen, req.u.data.length);
                        
                        char* new_buffer = NULL;
                        buflen = (req.u.data.length > buflen ? 
                                  req.u.data.length : buflen * 2);

                        /* There is a limit of 16 bits in the length */
                        if (buflen > G_MAXUINT16)
                                buflen = G_MAXUINT16;
                        new_buffer = g_realloc(buffer, buflen);
                        
                        buffer = new_buffer;
                        goto try_again;
                }                                 

                /* Check if results not available yet */
                if (errno == EAGAIN)
                {
                        DLOG_DEBUG("Not yet ready...");
                        if (counter-- > 0) {
                                sleep(1);
                                goto try_again;
                        }
                }
                
                DLOG_ERR("Get scan results failed");
                
                g_free(buffer);
                return -1;
        }
            
        if (req.u.data.length)
        {
                struct iw_event            iwe;
                struct stream_descr        stream;
                struct scan_results_t      *scan_results;
                struct wireless_iface      *wireless_if;
                int                        ret;
                gboolean                   wap_handled = FALSE;
                int                        we_version;
                
                scan_results = g_slice_new0(struct scan_results_t);
                memset(&iwe, 0, sizeof(iwe));
                
                if (ifindex != 0) {
                        wireless_if = get_interface_data(ifindex);
                        we_version = wireless_if->range.we_version_compiled;
                } else {
                        struct iw_range range;
                        if (iw_get_range_info(socket_open(), 
                                              wlan_status.ifname, 
                                              &range)<0)
                                memset(&range, 0, sizeof(range));
                        we_version = range.we_version_compiled;
                }
                
                iw_init_event_stream(&stream, buffer, req.u.data.length);
                do
                {
                        /* Extract an event */
                        ret = iw_extract_event_stream(
                                &stream, &iwe, 
                                we_version);
                        if (ret > 0) {
                                /* Let's peek what is coming so that we can
                                   separate different access points from
                                   the stream */

                                if (iwe.cmd == SIOCGIWAP) {
                                        /* Do not save if first time
                                           because WAP comes first, then other
                                           parameters */
                                        if (wap_handled == TRUE) {
                                                *scan_results_save = save_scan_results(scan_results, *scan_results_save);
                                                scan_results = g_slice_new0(struct scan_results_t);
                                        } else {
                                                wap_handled = TRUE;
                                        }
                                }
                                print_event_token(&iwe, scan_results, ifindex, 
                                                  TRUE);
                        }
                        
                }
                while (ret > 0);
                
                /* Check if the final results is still in the queue before
                   the result is sent into DBUS */
                if (wap_handled == TRUE) {
                        *scan_results_save = save_scan_results(
                                scan_results, 
                                *scan_results_save);
                } else {
                        // No results
                        g_slice_free(struct scan_results_t, scan_results);
                }
        }
        
        g_free(buffer);

        return 0;
}


gboolean ask_scan_results(int ifindex) 
{
        GSList *scan_results_save = NULL;
        dbus_int32_t number_of_results;
        
        if (scan_results_ioctl(ifindex, &scan_results_save) < 0)
                return FALSE;
        
        /* First send scan results if someone was expecting them */
        if (scan_name_cache != NULL) {
                number_of_results = g_slist_length(scan_results_save);
                
                /* Sort the list only if the amount of networks is very high and
                   we need to restrict the results */
                if (number_of_results > WLANCOND_MAX_NETWORKS)
                        scan_results_save = g_slist_sort(scan_results_save, 
                                                         network_compare);
                
                send_dbus_scan_results(scan_results_save, scan_name_cache, 
                                       number_of_results);
                g_free(scan_name_cache);
                scan_name_cache = NULL;
        }

        /* Try to associate if state is initialized_for_connection or
           signal level is low */
        if ((get_wlan_state() == WLAN_INITIALIZED_FOR_CONNECTION ||
             wlan_status.signal == WLANCOND_LOW) && 
            get_scan_state() == SCAN_ACTIVE) {
                
                DLOG_DEBUG("Connect from scan");
                
                set_scan_state(SCAN_NOT_ACTIVE);

                connect_from_scan_results(scan_results_save);
                
                return TRUE;
        }
        
        set_scan_state(SCAN_NOT_ACTIVE);
        
        if (get_wlan_state() == WLAN_INITIALIZED_FOR_SCAN &&
            power_down_after_scan == TRUE) {  
                set_interface_state(socket_open(), CLEAR, IFF_UP);
        }

        if (get_wlan_state() == WLAN_INITIALIZED_FOR_SCAN) {
                /* Save scan results temporarily */
                if (wlan_status.roam_cache) {
                        clean_roam_cache();
                }
                wlan_status.roam_cache = scan_results_save;                        
                set_wlan_state(WLAN_NOT_INITIALIZED, NO_SIGNAL, FORCE_NO);
        } else {
                clean_scan_results(&scan_results_save);
        }

        return TRUE;        
}

static DBusHandlerResult disconnect_request(DBusMessage    *message,
                                            DBusConnection *connection) {
        DBusMessage *reply;

        set_scan_state(SCAN_NOT_ACTIVE);

        /* Set_wlan_state puts IF down */
        set_wlan_state(WLAN_NOT_INITIALIZED, DISCONNECTED_SIGNAL, FORCE_YES);

        reply = new_dbus_method_return(message);
        send_and_unref(connection, reply);
        
        return DBUS_HANDLER_RESULT_HANDLED;
}
static DBusHandlerResult disassociate_request(DBusMessage    *message,
                                              DBusConnection *connection) {
        DBusMessage *reply;
        
        if (get_wlan_state() != WLAN_CONNECTED && 
            get_wlan_state() != WLAN_NO_ADDRESS) {
                DLOG_ERR("Not in correct state for disassociation");

                reply = new_dbus_method_return(message);
                send_and_unref(connection, reply);
                return DBUS_HANDLER_RESULT_HANDLED;
        }
        
        if (get_wpa_mode() == TRUE) {
                clear_wpa_keys(wlan_status.conn.bssid);
        }
        
        mlme_command(wlan_status.conn.bssid, IW_MLME_DISASSOC, 
                     WLANCOND_REASON_LEAVING);
        
        set_wlan_state(WLAN_INITIALIZED_FOR_CONNECTION, NO_SIGNAL, FORCE_NO);

        DLOG_DEBUG("Disassociated, trying to find a new connection");
        
        if (scan(wlan_status.conn.ssid, wlan_status.conn.ssid_len, TRUE) < 0) {
                /* Set_wlan_state puts IF down */
                set_wlan_state(WLAN_NOT_INITIALIZED, DISCONNECTED_SIGNAL,
                               FORCE_YES);
        }
        
        reply = new_dbus_method_return(message);
        send_and_unref(connection, reply);
        
        return DBUS_HANDLER_RESULT_HANDLED;
}
static DBusHandlerResult status_request(DBusMessage    *message,
                                        DBusConnection *connection) {
        DBusMessage *reply = NULL;
        struct iwreq req;
        struct iw_range range;
        char *essid = NULL;
        int essid_len;
        dbus_uint32_t sens = 0;
        dbus_uint32_t security = 0;
        dbus_uint32_t capability = 0;
        dbus_uint32_t channel = 0;
        unsigned char *bssid = NULL;
        //unsigned char *key = NULL;
        int sock;

        if (get_wlan_state() != WLAN_CONNECTED && 
            get_wlan_state() != WLAN_NO_ADDRESS &&
            get_mode() != WLANCOND_ADHOC) {
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                send_and_unref(connection, reply);
                return DBUS_HANDLER_RESULT_HANDLED;
        }
                
        sock = socket_open();

        init_iwreq(&req);
        
        essid = g_malloc0(IW_ESSID_MAX_SIZE+1);
        req.u.essid.pointer = (caddr_t)essid;
        req.u.essid.length = IW_ESSID_MAX_SIZE;
        req.u.essid.flags = 0;

        /* essid */
        if (ioctl(sock, SIOCGIWESSID, &req) < 0) {
                DLOG_ERR("Could not get ESSID");
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                goto param_err;
        }
        essid_len = req.u.essid.length;
        
        // Handle corner cases to keep the API the same
        if (essid_len == 0 || essid_len == 32)
                essid_len++;
        
        init_iwreq(&req);
        
        /* bssid */
        if (ioctl(sock, SIOCGIWAP, &req) < 0) {
                DLOG_ERR("Could not get BSSID");
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                goto param_err;
        }
        bssid = g_malloc(ETH_ALEN);
        memcpy(bssid, &req.u.ap_addr.sa_data, ETH_ALEN);

        init_iwreq(&req);
        struct iw_statistics stats;
        memset(&stats, 0, sizeof(struct iw_statistics));
        
        req.u.data.pointer = (caddr_t) &stats;
        req.u.data.length = sizeof(struct iw_statistics);
        req.u.data.flags = 1;
        
        /* Link quality i.e. stats */
        if (ioctl(sock, SIOCGIWSTATS, &req) < 0) {
                DLOG_ERR("Could not get statistics");
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                goto param_err;
        }
        sens = stats.qual.level - 0x100;
        
        /* Channel */
        init_iwreq(&req);
                
        if (ioctl(sock, SIOCGIWFREQ, &req) < 0) {
                DLOG_DEBUG("Could not get channel");
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                goto param_err;
        }
        
        if (iw_get_range_info(sock, wlan_status.ifname, &range) >= 0) {
                double freq = iw_freq2float(&(req.u.freq));
                channel = iw_freq_to_channel(freq, &range);
        }
        
        if (channel < WLANCOND_MIN_WLAN_CHANNEL || 
            channel > WLANCOND_MAX_WLAN_CHANNEL) {
                channel = 0;
                DLOG_DEBUG("Got invalid channel\n");
        }
        
        /* Mode (Adhoc/Infra) */
        init_iwreq(&req);

        if (ioctl(sock, SIOCGIWMODE, &req) < 0) {
                DLOG_ERR("Could not get operating mode");
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                goto param_err;
        }

        if (req.u.mode == IW_MODE_ADHOC) {
                capability |= WLANCOND_ADHOC;
        } else if (req.u.mode == IW_MODE_INFRA) {
                capability |= WLANCOND_INFRA;
        }        
        
        init_iwreq(&req);

        /* encryption status */
        security = wlan_status.conn.encryption;

#if 0
        key = g_malloc(IW_ENCODING_TOKEN_MAX);
        req.u.data.pointer = (caddr_t) key;
        req.u.data.length = IW_ENCODING_TOKEN_MAX;
        req.u.data.flags = 0;
        
        if (ioctl(sock, SIOCGIWENCODE, &req) < 0) {
                reply = new_dbus_error(message, WLANCOND_ERROR_IOCTL_FAILED);
                goto param_err;
        } 
        
        if (req.u.data.flags & IW_ENCODE_OPEN)
                security |= WLANCOND_OPEN;
        
        if (req.u.data.flags & IW_ENCODE_RESTRICTED)
                security |= WLANCOND_WEP;
        
        /* Currently we don't know if EAP or PSK is in use */
        if (req.u.data.flags & IW_ENCODE_TKIP)
                security |= WLANCOND_WPA_PSK & WLANCOND_WPA_EAP & 
                        WLANCOND_WPA_TKIP;
        
        if (req.u.data.flags & IW_ENCODE_AES)
                security |= WLANCOND_WPA_PSK & WLANCOND_WPA_EAP &
                        WLANCOND_WPA_AES;
        
        g_free(key);
#endif        
        init_iwreq(&req);

        /* Speed / Rate */
        if (ioctl(sock, SIOCGIWRATE, &req) < 0) {
                DLOG_ERR("Could not get the rate");
        } 
        capability |= req.u.bitrate.value;
        
        reply = new_dbus_method_return(message);
        
        gchar* ifname = wlan_status.ifname;

        append_dbus_args(reply,
                         DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, 
                         &essid, essid_len,
                         DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &bssid, ETH_ALEN,
                         DBUS_TYPE_UINT32, &sens,
                         DBUS_TYPE_UINT32, &channel,
                         DBUS_TYPE_UINT32, &capability,
                         DBUS_TYPE_UINT32, &security,
                         DBUS_TYPE_STRING, &ifname,
                         DBUS_TYPE_INVALID);

        send_and_unref(connection, reply);

        g_free(essid);
        g_free(bssid);
        
        return DBUS_HANDLER_RESULT_HANDLED;
        
  param_err:
        g_free(essid);
        g_free(bssid);
        //g_free(key);
        if (reply == NULL) {
                DLOG_DEBUG("Parameter error in status request");
                reply = new_dbus_error(message, DBUS_ERROR_INVALID_ARGS);
        }
        send_and_unref(connection, reply);
        return DBUS_HANDLER_RESULT_HANDLED;
}

static DBusHandlerResult interface_request(DBusMessage    *message,
                                           DBusConnection *connection) {
        DBusMessage *reply;
        gchar* ifname = wlan_status.ifname;

        reply = new_dbus_method_return(message);
        
        append_dbus_args(reply,
                         DBUS_TYPE_STRING, &ifname,
                         DBUS_TYPE_INVALID);        
        
        send_and_unref(connection, reply);
        
        return DBUS_HANDLER_RESULT_HANDLED;
}

static DBusHandlerResult connection_status_request(
        DBusMessage    *message,
        DBusConnection *connection) {
        
        DBusMessage *reply;
        dbus_bool_t state = FALSE;
        
        guint state_v = get_wlan_state();

        if (state_v == WLAN_INITIALIZED ||
            state_v == WLAN_NO_ADDRESS ||
            state_v == WLAN_CONNECTED)
                state = TRUE;
        
        reply = new_dbus_method_return(message);
        
        append_dbus_args(reply,
                         DBUS_TYPE_BOOLEAN, &state,
                         DBUS_TYPE_INVALID);        

        send_and_unref(connection, reply);
        
        return DBUS_HANDLER_RESULT_HANDLED;
}
static DBusHandlerResult set_pmksa_request(DBusMessage    *message,
                                           DBusConnection *connection) {
        
        DBusMessage *reply = NULL;
        unsigned int pmkid_len, mac_len;
        unsigned char *pmkid;
        unsigned char *mac;
        dbus_uint32_t action;
        DBusError derror;

        dbus_error_init(&derror);
        
        if (dbus_message_get_args(
                    message, &derror,        
                    DBUS_TYPE_UINT32, &action,
                    DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &pmkid, &pmkid_len,
                    DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &mac, &mac_len,
                    DBUS_TYPE_INVALID) == FALSE) 
        {
                DLOG_ERR("Failed to parse set_pmksa request: %s",
                         derror.message);
                dbus_error_free(&derror);
                goto param_err;
        }
        
        if (action != IW_PMKSA_ADD) {
                DLOG_ERR("Invalid action");
                goto param_err;
        }
        
        if (pmkid == NULL || pmkid_len != WLANCOND_PMKID_LEN || mac == NULL 
            || mac_len != ETH_ALEN) {
                DLOG_ERR("Invalid arguments");
                goto param_err;
        }
        
        add_to_pmksa_cache(pmkid, mac);
        
        print_mac("PMKSA added successfully for address:", mac);
        
        reply = new_dbus_method_return(message);
        send_and_unref(connection, reply);
        
        return DBUS_HANDLER_RESULT_HANDLED;
        
  param_err:
        if (reply == NULL) {
                DLOG_DEBUG("Parameter error in set_pmksa");
                reply = new_dbus_error(message, DBUS_ERROR_INVALID_ARGS);
        }
        send_and_unref(connection, reply);
        return DBUS_HANDLER_RESULT_HANDLED;
} 

static DBusHandlerResult set_powersave_request(DBusMessage    *message,
                                               DBusConnection *connection) {
        
        DBusMessage *reply = NULL;
        int sock;
        DBusError error;
        dbus_bool_t onoff;
        
        sock = socket_open();
        
        dbus_error_init(&error);
        
        if (dbus_message_get_args(message, &error,
                                  DBUS_TYPE_BOOLEAN, &onoff,
                                  DBUS_TYPE_INVALID) == FALSE) {
                DLOG_ERR("Failed to parse message: %s",
                         error.message);
                dbus_error_free(&error);
                
                send_invalid_args(connection, message);
                return DBUS_HANDLER_RESULT_HANDLED;
        }
        
        /* Powersave can be allowed only when we are properly connected
           or when the entity asking for connection wants powersave despite of
           the state */
        if (onoff == TRUE) {
                if (get_wlan_state() == WLAN_NOT_INITIALIZED) {
                        set_wlan_state(WLAN_NOT_INITIALIZED, NO_SIGNAL, FORCE_YES);
                } else if (get_wlan_state() != WLAN_NO_ADDRESS ||
                           (connect_name_cache != NULL && 
                            strcmp(dbus_message_get_sender(message), 
                                   connect_name_cache) == 0)) {
                        if (set_power_state(powersave, sock) == FALSE) {
                                DLOG_ERR("Setting powersave failed");
                                // Not fatal
                        }
                }
        } else {
                // Go to full power
                if (set_power_state(WLANCOND_POWER_ON, sock) == FALSE) {
                        DLOG_ERR("Setting powersave failed");
                        // Not fatal
                }
        }
        
        DLOG_DEBUG("WLAN powersave %s", onoff==TRUE?"on":"off");
        
        reply = new_dbus_method_return(message);
        send_and_unref(connection, reply);
        
        return DBUS_HANDLER_RESULT_HANDLED;
}

static void wpa_ie_push_cb(DBusPendingCall *pending, 
                           void *user_data)
{
        DBusMessage *reply;
        DBusError error;

        //DLOG_DEBUG("WPA IE callback");
        
        dbus_error_init (&error);
        
        reply = dbus_pending_call_steal_reply(pending);
        
        if (dbus_set_error_from_message(&error, reply)) {
                
                DLOG_DEBUG("EAP WPA IE push call result:%s", error.name);
                
                dbus_error_free(&error);
                
                set_wlan_state(WLAN_NOT_INITIALIZED,
                               DISCONNECTED_SIGNAL,
                               FORCE_YES);
        }
        
        if (reply)
                dbus_message_unref(reply);
        dbus_pending_call_unref(pending);
}

int wpa_ie_push(unsigned char* ap_mac_addr, unsigned char* ap_wpa_ie,
                int ap_wpa_ie_len, char* ssid, int ssid_len, 
                unsigned int authentication_type) {

        DBusMessage *msg;
        DBusPendingCall *pending;

        if (authentication_type != EAP_AUTH_TYPE_WFA_SC) {
                if (wlan_status.wpa_ie.ie_len == 0 || ap_wpa_ie == NULL || 
                    ssid == NULL) {
                        DLOG_ERR("WPA IE / SSID (%s) not valid", ssid);
                        return -1;
                }
        }

        msg = dbus_message_new_method_call(
                EAP_SERVICE,
                EAP_REQ_PATH,
                EAP_REQ_INTERFACE,
                EAP_WPA_IE_PUSH_REQ);
        
        if (msg == NULL) {
                return -1;
        }
        
        append_dbus_args(
                msg,
                DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, 
                &wlan_status.wpa_ie.ie, wlan_status.wpa_ie.ie_len,
                DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, &ap_wpa_ie, 
                ap_wpa_ie_len,
                DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, 
                &ssid, ssid_len,
                DBUS_TYPE_UINT32, &wlan_status.pairwise_cipher,
                DBUS_TYPE_UINT32, &wlan_status.group_cipher,
                DBUS_TYPE_ARRAY, DBUS_TYPE_BYTE, 
                &ap_mac_addr, ETH_ALEN,
                DBUS_TYPE_UINT32, &authentication_type,
                DBUS_TYPE_INVALID);

        if (!dbus_connection_send_with_reply(get_dbus_connection(), 
                                             msg, &pending, -1))
                die("Out of memory");
        
        if (!dbus_pending_call_set_notify (pending, wpa_ie_push_cb, NULL, NULL))
                die("Out of memory");
        
        dbus_message_unref(msg);
        
        return 0;
}
int wpa_mic_failure_event(dbus_bool_t key_type, dbus_bool_t is_fatal) {
        DBusMessage *msg;
        DBusMessage *reply;
        DBusError derr;

        msg = dbus_message_new_method_call(
                EAP_SERVICE,
                EAP_REQ_PATH,
                EAP_REQ_INTERFACE,
                EAP_WPA_MIC_FAILURE_REQ);
        
        if (msg == NULL) {
                return -1;
        }
        
        append_dbus_args(msg,
                         DBUS_TYPE_BOOLEAN, &key_type, 
                         DBUS_TYPE_BOOLEAN, &is_fatal, 
                         DBUS_TYPE_INVALID);
        
        dbus_error_init(&derr);
        
        reply = dbus_connection_send_with_reply_and_block(
                get_dbus_connection(), msg, -1, &derr);
        
        dbus_message_unref(msg);
        
        if (dbus_error_is_set(&derr)) {
                DLOG_ERR("EAP returned error: %s", derr.name);
                
                dbus_error_free(&derr);
                if (reply)
                        dbus_message_unref(reply);
                return -1;
        }
        
        dbus_message_unref(reply);
        
        return 0;
}
int associate_supplicant(void) {
        DBusMessage *msg;
        DBusMessage *reply;
        DBusError derr;
        
        msg = dbus_message_new_method_call(
                EAP_SERVICE,
                EAP_REQ_PATH,
                EAP_REQ_INTERFACE,
                EAP_ASSOCIATE_REQ);
        
        if (msg == NULL) {
                return -1;
        }
        
        dbus_error_init(&derr);
        
        reply = dbus_connection_send_with_reply_and_block(
                get_dbus_connection(), msg, -1, &derr);
        
        dbus_message_unref(msg);
        
        if (dbus_error_is_set(&derr)) {
                DLOG_ERR("EAP returned error: %s", derr.name);
                
                dbus_error_free(&derr);
                if (reply)
                        dbus_message_unref(reply);
                return -1;
        }
        
        dbus_message_unref(reply);
        
        return 0;
}
static void disassociate_cb(DBusPendingCall *pending, 
                            void *user_data)
{
        DBusMessage *reply;
        DBusError error;
        
        dbus_error_init (&error);
        
        reply = dbus_pending_call_steal_reply(pending);
        
        if (dbus_set_error_from_message(&error, reply)) {
                
                DLOG_DEBUG("EAP disassociate call result:%s", error.name);
                
                dbus_error_free(&error);
        }
        
        if (reply)
                dbus_message_unref(reply);
        dbus_pending_call_unref(pending);
}

int disassociate_eap(void) {
        DBusMessage *msg;
        DBusPendingCall *pending;
        
        msg = dbus_message_new_method_call(
                EAP_SERVICE,
                EAP_REQ_PATH,
                EAP_REQ_INTERFACE,
                EAP_DISASSOCIATE_REQ);
        
        if (msg == NULL) {
                return -1;
        }
        
        if (!dbus_connection_send_with_reply(get_dbus_connection(), 
                                             msg, &pending, -1))
                die("Out of memory");
        
        if (!dbus_pending_call_set_notify (pending, disassociate_cb, NULL, 
                                           NULL))
                die("Out of memory");
        
        dbus_message_unref(msg);
        
        return 0;
}

typedef DBusHandlerResult (*handler_func)(DBusMessage *message,
                                          DBusConnection *connection);

typedef struct {
        const char *interface;
        const char *name;
        handler_func func;
} method_handler_t;

static method_handler_t handlers[] = {
        { WLANCOND_REQ_INTERFACE, WLANCOND_SETTINGS_AND_CONNECT_REQ, settings_and_connect_request},
        { WLANCOND_REQ_INTERFACE, WLANCOND_SCAN_REQ, scan_request},        
        { WLANCOND_REQ_INTERFACE, WLANCOND_STATUS_REQ, status_request},        
        { WLANCOND_REQ_INTERFACE, WLANCOND_INTERFACE_REQ, interface_request},        
        { WLANCOND_REQ_INTERFACE, WLANCOND_CONNECTION_STATUS_REQ, connection_status_request},        
        { WLANCOND_REQ_INTERFACE, WLANCOND_SET_PMKSA_REQ, set_pmksa_request},
        { WLANCOND_REQ_INTERFACE, WLANCOND_SET_POWERSAVE_REQ, set_powersave_request},
        { WLANCOND_REQ_INTERFACE, WLANCOND_DISCONNECT_REQ, disconnect_request},
        { WLANCOND_REQ_INTERFACE, WLANCOND_DISASSOCIATE_REQ, disassociate_request},
        { NULL }
};

DBusHandlerResult wlancond_req_handler(DBusConnection     *connection,
                                       DBusMessage        *message,
                                       void               *user_data) {
        method_handler_t *handler;

/*        DLOG_DEBUG("Received %s.%s",
                   dbus_message_get_interface(message),
                   dbus_message_get_member(message));
*/

#ifdef USE_MCE_MODE        
        if (dbus_message_is_signal(message,
                                   MCE_SIGNAL_IF,
                                   MCE_DEVICE_MODE_SIG)) {
                return mode_change_dbus(message);
        }
#ifdef ACTIVITY_CHECK
        if (dbus_message_is_signal(message,
                                   MCE_SIGNAL_IF,
                                   MCE_INACTIVITY_SIG)) {
                return activity_check_dbus(message);
        }
#endif
#endif 
        if (dbus_message_is_signal(message,
                                   ICD_DBUS_INTERFACE,
                                   ICD_STATUS_CHANGED_SIG))
                return icd_check_signal_dbus(message);
        
        /* The rest should be just method calls */
        if (dbus_message_get_type(message) != DBUS_MESSAGE_TYPE_METHOD_CALL) {
                  return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
        }
        
        for (handler = handlers; handler->interface != NULL; handler++) {
                if (dbus_message_is_method_call(message,
                                                handler->interface,
                                                handler->name)) {
                        DLOG_DEBUG("Received %s", handler->name);
                        return handler->func(message, connection);
                }
        }
        return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
}

static DBusObjectPathVTable wlancond_req_vtable = {
        .message_function    = wlancond_req_handler,
        .unregister_function = NULL
};


void init_dbus_handlers(DBusConnection *connection) {
        dbus_bool_t ret;
        ret = dbus_connection_register_object_path(connection,
                                                   WLANCOND_REQ_PATH,
                                                   &wlancond_req_vtable,
                                                   NULL);
        if (ret == FALSE) {
                DLOG_ERR("dbus_connection_register_object_path failed");
        }
#ifdef USE_MCE_MODE
        if (!add_mode_listener(connection)) {
                DLOG_ERR("Adding mode listener failed");
        }
#endif
        if (!add_icd_listener(connection)) {
                DLOG_ERR("Adding icd listener failed");
        }  
}

void destroy_dbus_handlers(DBusConnection *connection) {
        dbus_connection_unregister_object_path(connection, WLANCOND_REQ_PATH);
}

---