src/wifi-netlink-scan.c

   1 /*
   2  * Network Configuration Module
   3  *
   4  * Copyright (c) 2000 - 2012 Samsung Electronics Co., Ltd. All rights reserved.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  * http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  */
  19
  20 #include <glib.h>
  21
  22 #include "netsupplicant.h"
  23 #include "log.h"
  24 #include "util.h"
  25 #include "wifi-config.h"
  26 #include "wifi-netlink-scan.h"
  27 #include <netlink/genl/genl.h>
  28 #include <netlink/genl/family.h>
  29 #include <netlink/genl/ctrl.h>
  30 #include <netlink/msg.h>
  31 #include <netlink/attr.h>
  32 #include <netlink/netlink.h>
  33 #include <ctype.h>
  34
  35 static GSList *bss_info_list = NULL;
  36
  37 void __netconfig_notify_netlink_scan_done(void)
  38 {
  39         GVariantBuilder *builder = NULL;
  40         GVariantBuilder *builder1 = NULL;
  41         GVariantBuilder *builder2 = NULL;
  42         GSList* list = NULL;
  43         const char *prop_ssid = "ssid";
  44         const char *prop_bssid = "bssid";
  45         const char *prop_freq = "freq";
  46         const char *prop_rssi = "rssi";
  47         const char *prop_vsie_list = "vsie_list";
  48         const char *prop_sec = "security";
  49         const char *prop_enc = "encryption";
  50
  51         builder = g_variant_builder_new(G_VARIANT_TYPE("a{sv}"));
  52         for (list = bss_info_list; list != NULL; list = list->next) {
  53                 struct bss_scan_info_t *bss_info = (struct bss_scan_info_t *)list->data;
  54
  55                 if (bss_info) {
  56                         char *bssid = (char *)bss_info->bssid;
  57                         char *ssid = (char *)bss_info->ssid;
  58                         GSList *vsie_list = bss_info->vsie_list;
  59                         int freq = (int)bss_info->freq;
  60                         int signal = (int)bss_info->signal;
  61                         int sec_type = (int)bss_info->security_type;
  62                         int enc_type = (int)bss_info->encryption_type;
  63
  64                         g_variant_builder_add(builder, "{sv}", prop_ssid, g_variant_new_string(ssid));
  65                         g_variant_builder_add(builder, "{sv}", prop_bssid, g_variant_new_string(bssid));
  66                         g_variant_builder_add(builder, "{sv}", prop_freq, g_variant_new_int32(freq));
  67                         g_variant_builder_add(builder, "{sv}", prop_rssi, g_variant_new_int32(signal));
  68                         builder1 = g_variant_builder_new(G_VARIANT_TYPE("a{sv}"));
  69                         GSList *list;
  70                         unsigned char *net_vsie;
  71                         unsigned int net_vsie_len;
  72                         int count;
  73                         for (list = vsie_list; list; list = list->next) {
  74                                 builder2 = g_variant_builder_new(G_VARIANT_TYPE("ay"));
  75                                 net_vsie = (unsigned char *)list->data;
  76                                 net_vsie_len = net_vsie[1] + 2;
  77
  78                                 for (count = 0; count < net_vsie_len; count++) {
  79                                         g_variant_builder_add(builder2, "y", net_vsie[count]);
  80                                 }
  81
  82                                 g_variant_builder_add(builder1, "{sv}", "Vsie", g_variant_builder_end(builder2));
  83                                 g_variant_builder_unref(builder2);
  84                         }
  85                         g_variant_builder_add(builder, "{sv}", prop_vsie_list, g_variant_builder_end(builder1));
  86                         g_variant_builder_unref(builder1);
  87
  88                         if (vsie_list != NULL)
  89                                 g_slist_free_full(vsie_list, g_free);
  90
  91                         vsie_list = NULL;
  92
  93                         g_variant_builder_add(builder, "{sv}", prop_sec, g_variant_new_int32(sec_type));
  94                         g_variant_builder_add(builder, "{sv}", prop_enc, g_variant_new_int32(enc_type));
  95                 }
  96         }
  97
  98         wifi_emit_netlink_scan_completed((Wifi *)get_wifi_object(), g_variant_builder_end(builder));
  99         g_variant_builder_unref(builder);
 100
 101         if (bss_info_list != NULL)
 102                 g_slist_free_full(bss_info_list, g_free);
 103
 104         bss_info_list = NULL;
 105         INFO("NetlinkScanCompleted");
 106
 107         return;
 108 }
 109
 110 static int ack_handler(struct nl_msg *msg, void *user_data)
 111 {
 112         int *err = user_data;
 113         *err = 0;
 114         return NL_STOP;
 115 }
 116
 117 static int finish_handler(struct nl_msg *msg, void *user_data)
 118 {
 119         int *ret = user_data;
 120         *ret = 0;
 121         return NL_SKIP;
 122 }
 123
 124 static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
 125                          void *user_data)
 126 {
 127         int *ret = user_data;
 128         *ret = err->error;
 129         return NL_SKIP;
 130 }
 131
 132 static int no_seq_check(struct nl_msg *msg, void *user_data)
 133 {
 134         DBG("");
 135         return NL_OK;
 136 }
 137
 138 static int __netconfig_family_handler(struct nl_msg *msg, void *user_data)
 139 {
 140         /** Callback for NL_CB_VALID in multicast group */
 141         struct netconfig_netlink_scan_handler_args *grp = user_data;
 142         struct nlattr *tb[CTRL_ATTR_MAX + 1];
 143         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 144         struct nlattr *mc_grp;
 145         int rem_mc_grp;
 146
 147         nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
 148
 149         if (!tb[CTRL_ATTR_MCAST_GROUPS])
 150                 return NL_SKIP;
 151
 152         nla_for_each_nested(mc_grp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mc_grp) {
 153                 struct nlattr *tb_mc_grp[CTRL_ATTR_MCAST_GRP_MAX + 1];
 154
 155                 nla_parse(tb_mc_grp, CTRL_ATTR_MCAST_GRP_MAX, nla_data(mc_grp), nla_len(mc_grp), NULL);
 156
 157                 if (!tb_mc_grp[CTRL_ATTR_MCAST_GRP_NAME] || !tb_mc_grp[CTRL_ATTR_MCAST_GRP_ID])
 158                         continue;
 159                 if (strncmp(nla_data(tb_mc_grp[CTRL_ATTR_MCAST_GRP_NAME]), grp->group,
 160                                         nla_len(tb_mc_grp[CTRL_ATTR_MCAST_GRP_NAME])))
 161                         continue;
 162
 163                 grp->id = nla_get_u32(tb_mc_grp[CTRL_ATTR_MCAST_GRP_ID]);
 164                 break;
 165         }
 166
 167         return NL_SKIP;
 168 }
 169
 170 static int __netconfig_get_multicast_id(struct nl_sock *socket, const char *family, const char *group)
 171 {
 172         struct nl_msg *msg = NULL;
 173         struct nl_cb *cb = NULL;
 174         int ret, ctrl_id;
 175         struct netconfig_netlink_scan_handler_args grp = { .group = group, .id = -ENOENT, };
 176
 177         msg = nlmsg_alloc();
 178         if (!msg)
 179                 return -ENOMEM;
 180
 181         cb = nl_cb_alloc(NL_CB_DEFAULT);
 182         if (!cb) {
 183                 ret = -ENOMEM;
 184                 goto cb_fail;
 185         }
 186
 187         ctrl_id = genl_ctrl_resolve(socket, "nlctrl");
 188
 189         genlmsg_put(msg, 0, 0, ctrl_id, 0, 0, CTRL_CMD_GETFAMILY, 0);
 190
 191         ret = -ENOBUFS;
 192         NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, family);
 193
 194         ret = nl_send_auto_complete(socket, msg);
 195         if (ret < 0)
 196                 goto out;
 197
 198         ret = 1;
 199
 200         nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &ret);
 201         nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &ret);
 202         nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, __netconfig_family_handler, &grp);
 203
 204         while (ret > 0)
 205                 nl_recvmsgs(socket, cb);
 206
 207         if (ret == 0)
 208                 ret = grp.id;
 209
 210 nla_put_failure:
 211 out:
 212         nl_cb_put(cb);
 213 cb_fail:
 214         nlmsg_free(msg);
 215         return ret;
 216 }
 217
 218 static void __netconfig_macaddress_str(char *bssid, unsigned char *user_data)
 219 {
 220         int i;
 221
 222         for (i = 0; i < 6; i++) {
 223                 if (i == 0) {
 224                         snprintf(bssid, 3, "%02x", user_data[i]);
 225                         bssid += 2;
 226                 } else {
 227                         snprintf(bssid, 4, ":%02x", user_data[i]);
 228                         bssid += 3;
 229                 }
 230         }
 231 }
 232
 233 typedef enum {
 234         WIFI_SECURITY_TYPE_NONE = 0,
 235         WIFI_SECURITY_TYPE_WEP = 1,
 236         WIFI_SECURITY_TYPE_WPA_PSK = 2,
 237         WIFI_SECURITY_TYPE_WPA2_PSK = 3,
 238         WIFI_SECURITY_TYPE_EAP = 4,
 239 } wifi_security_type_e;
 240
 241 typedef enum {
 242         WIFI_ENCRYPTION_TYPE_NONE = 0,
 243         WIFI_ENCRYPTION_TYPE_WEP = 1,
 244         WIFI_ENCRYPTION_TYPE_TKIP = 2,
 245         WIFI_ENCRYPTION_TYPE_AES = 3,
 246         WIFI_ENCRYPTION_TYPE_TKIP_AES_MIXED = 4,
 247 } wifi_encryption_type_e;
 248
 249 static unsigned char ms_oui[3]      = { 0x00, 0x50, 0xf2 };
 250 static unsigned char ieee80211_oui[3]   = { 0x00, 0x0f, 0xac };
 251
 252 static void __netconfig_get_security(unsigned char *bss_element, int length, wifi_security_type_e *sec_type, wifi_encryption_type_e *enc_type)
 253 {
 254         int i;
 255         unsigned char *data;
 256         uint8_t *t_data;
 257         int len;
 258         __u16 count;
 259
 260         *sec_type = WIFI_SECURITY_TYPE_NONE;
 261         *enc_type = WIFI_ENCRYPTION_TYPE_NONE;
 262
 263         while (length >= 2 && length >= bss_element[1]) {
 264                 if (bss_element[0] == 221 && (bss_element[1] >= 4 && memcmp(bss_element + 2, ms_oui, 3) == 0)) {
 265                         data = bss_element + 2 + 4 + 2 + 4;
 266                         len = bss_element[1] - 4 - 2 - 4;
 267
 268                         if (len < 2) {
 269                                 length -= bss_element[1] + 2;
 270                                 bss_element += bss_element[1] + 2;
 271                                 continue;
 272                         }
 273
 274                         count = data[0] | (data[1] << 8);
 275
 276                         if (2 + (count * 4) > len) {
 277                                 length -= bss_element[1] + 2;
 278                                 bss_element += bss_element[1] + 2;
 279                                 continue;
 280                         }
 281
 282                         for (i = 0; i < count; i++) {
 283                                 t_data = (data + 2 + (i * 4));
 284
 285                                 if ((memcmp(t_data, ms_oui, 3) == 0) || (memcmp(t_data, ieee80211_oui, 3) == 0)) {
 286                                         if (t_data[3] == 1 || t_data[3] == 5) { // 1 : WEP-40, 5 : WEP-104
 287                                                 *sec_type = WIFI_SECURITY_TYPE_WEP;
 288                                                 *enc_type = WIFI_ENCRYPTION_TYPE_WEP;
 289                                                 return;
 290                                         } else if (t_data[3] == 2) { // 2 : TKIP
 291                                                 if (*sec_type != WIFI_SECURITY_TYPE_WPA2_PSK)
 292                                                         *sec_type = WIFI_SECURITY_TYPE_WPA_PSK;
 293                                                 if (*enc_type == WIFI_ENCRYPTION_TYPE_AES)
 294                                                         *enc_type = WIFI_ENCRYPTION_TYPE_TKIP_AES_MIXED;
 295                                                 else
 296                                                         *enc_type = WIFI_ENCRYPTION_TYPE_TKIP;
 297                                         } else if (t_data[3] == 4) { // 3 : CCMP
 298                                                 if (*sec_type != WIFI_SECURITY_TYPE_WPA2_PSK)
 299                                                         *sec_type = WIFI_SECURITY_TYPE_WPA_PSK;
 300                                                 if (*enc_type == WIFI_ENCRYPTION_TYPE_TKIP)
 301                                                         *enc_type = WIFI_ENCRYPTION_TYPE_TKIP_AES_MIXED;
 302                                                 else
 303                                                         *enc_type = WIFI_ENCRYPTION_TYPE_AES;
 304                                         }
 305                                 } //if
 306                         } //for
 307
 308                         data += 2 + (count * 4);
 309                         len -= 2 + (count * 4);
 310
 311                         if (len < 2) {
 312                                 length -= bss_element[1] + 2;
 313                                 bss_element += bss_element[1] + 2;
 314                                 continue;
 315                         }
 316
 317                         count = data[0] | (data[1] << 8);
 318
 319                         if (2 + (count * 4) > len) {
 320                                 length -= bss_element[1] + 2;
 321                                 bss_element += bss_element[1] + 2;
 322                                 continue;
 323                         }
 324
 325                         for (i = 0; i < count; i++) {
 326                                 t_data = (data + 2 + (i * 4));
 327
 328                                 if ((memcmp(t_data, ms_oui, 3) == 0) || (memcmp(t_data, ieee80211_oui, 3) == 0)) {
 329                                         if (t_data[3] == 1 || t_data[3] == 3 || t_data[3] == 5) { // 1 : IEEE 802.1X, 3 : FT/IEEE 802.1X, 5 : IEEE 802.1X/SHA-256
 330                                                 *sec_type = WIFI_SECURITY_TYPE_EAP;
 331                                         } else if (t_data[3] == 2 || t_data[3] == 4 || t_data[3] == 6) {  // 2 : PSK, 4 : FT/PSK, 6 : PSK/SHA-256
 332                                                 if (*sec_type != WIFI_SECURITY_TYPE_WPA2_PSK)
 333                                                         *sec_type = WIFI_SECURITY_TYPE_WPA_PSK;
 334                                         }
 335                                 }
 336                         }
 337                 } else if (bss_element[0] == 48 && (bss_element[1] >= 2 && bss_element[1] <= 255)) {
 338                         data = bss_element + 2 + 2 + 4;
 339                         len = bss_element[1] - 2 - 4;
 340
 341                         if (len < 2) {
 342                                 length -= bss_element[1] + 2;
 343                                 bss_element += bss_element[1] + 2;
 344                                 continue;
 345                         }
 346
 347                         count = data[0] | (data[1] << 8);
 348                         if (2 + (count + 4) > len) {
 349                                 length -= bss_element[1] + 2;
 350                                 bss_element += bss_element[1] + 2;
 351                                 continue;
 352                         }
 353
 354                         for (i = 0; i < count; i++) {
 355                                 t_data = (data + 2 + (i * 4));
 356
 357                                 if ((memcmp(t_data, ms_oui, 3) == 0) || (memcmp(t_data, ieee80211_oui, 3) == 0)) {
 358                                         if (t_data[3] == 1 || t_data[3] == 5) { // 1 : WEP-40, 5 : WEP-104
 359                                                 *sec_type = WIFI_SECURITY_TYPE_WEP;
 360                                                 *enc_type = WIFI_ENCRYPTION_TYPE_WEP;
 361                                                 return;
 362                                         } else if (t_data[3] == 2) { // 2 : TKIP
 363                                                 *sec_type = WIFI_SECURITY_TYPE_WPA2_PSK;
 364                                                 if (*enc_type == WIFI_ENCRYPTION_TYPE_AES)
 365                                                         *enc_type = WIFI_ENCRYPTION_TYPE_TKIP_AES_MIXED;
 366                                                 else
 367                                                         *enc_type = WIFI_ENCRYPTION_TYPE_TKIP;
 368                                         } else if (t_data[3] == 4) { // 3 : CCMP
 369                                                 *sec_type = WIFI_SECURITY_TYPE_WPA2_PSK;
 370                                                 if (*enc_type == WIFI_ENCRYPTION_TYPE_TKIP)
 371                                                         *enc_type = WIFI_ENCRYPTION_TYPE_TKIP_AES_MIXED;
 372                                                 else
 373                                                         *enc_type = WIFI_ENCRYPTION_TYPE_AES;
 374                                         }
 375                                 }
 376                         }
 377
 378                         data += 2 + (count * 4);
 379                         len -= 2 + (count * 4);
 380
 381                         if (len < 2) {
 382                                 length -= bss_element[1] + 2;
 383                                 bss_element += bss_element[1] + 2;
 384                                 continue;
 385                         }
 386
 387                         count = data[0] | (data[1] << 8);
 388                         if (2 + (count * 4) > len) {
 389                                 length -= bss_element[1] + 2;
 390                                 bss_element += bss_element[1] + 2;
 391                                 continue;
 392                         }
 393
 394                         for (i = 0; i < count; i++) {
 395                                 t_data = (data + 2 + (i * 4));
 396
 397                                 if ((memcmp(t_data, ms_oui, 3) == 0) || (memcmp(t_data, ieee80211_oui, 3) == 0)) {
 398                                         if (t_data[3] == 1 || t_data[3] == 3 || t_data[3] == 5) { // 1 : IEEE 802.1X, 3 : FT/IEEE 802.1X, 5 : IEEE 802.1X/SHA-256
 399                                                 *sec_type = WIFI_SECURITY_TYPE_EAP;
 400                                         } else if (t_data[3] == 2 || t_data[3] == 4 || t_data[3] == 6) {  // 2 : PSK, 4 : FT/PSK, 6 : PSK/SHA-256
 401                                                 *sec_type = WIFI_SECURITY_TYPE_WPA2_PSK;
 402                                         }
 403                                 }
 404                         } //for
 405                 } //else if
 406
 407                 length -= bss_element[1] + 2;
 408                 bss_element += bss_element[1] + 2;
 409         } //while
 410
 411         return;
 412 }
 413
 414 static void __netconfig_get_vsie(unsigned char *bss_element, int length, GSList **dst)
 415 {
 416         unsigned char *vsie;
 417         int vsie_len = 0;
 418
 419         if (length < 3) {
 420                 DBG("Vendor specific data not available");
 421                 return;
 422         }
 423
 424         while (length >= 2 && length >= bss_element[1]) {
 425                 if (bss_element[0] == NETCONFIG_VENDOR_SPECIFIC_ID) {
 426                         vsie_len = bss_element[1]+2;
 427                         vsie = (unsigned char *)g_try_malloc0(vsie_len);
 428
 429                         if (vsie) {
 430                                 memcpy(vsie, bss_element, vsie_len);
 431                                 *dst = g_slist_append(*dst, vsie);
 432                         } else
 433                                 DBG("Failed to allocate memory");
 434                 }
 435
 436                 length -= bss_element[1] + 2;
 437                 bss_element += bss_element[1] + 2;
 438         }
 439 }
 440
 441 static void __netconfig_found_ap(unsigned char *bss_element, int length, char *str)
 442 {
 443         uint8_t len;
 444         uint8_t *data;
 445         int i;
 446
 447         while (length >= 2 && length >= bss_element[1]) {
 448                 if (bss_element[0] == 0 && bss_element[1] <= 32) {
 449                         len = bss_element[1];
 450                         data = bss_element + 2;
 451                         for (i = 0; i < len; i++) {
 452                                 if (isprint(data[i]) && data[i] != ' ' && data[i] != '\\')
 453                                         snprintf(&str[i], 2, "%c", data[i]);
 454                                 else if (data[i] == ' ' && (i != 0 && i != len -1))
 455                                         snprintf(&str[i], 2, "%c", ' ');
 456                                 else
 457                                         snprintf(&str[i], 2, "%c", data[i]);
 458                         }
 459                         break;
 460                 }
 461                 length -= bss_element[1] + 2;
 462                 bss_element += bss_element[1] + 2;
 463         }
 464 }
 465
 466 static int __netconfig_netlink_scan_cb(struct nl_msg *msg, void *user_data)
 467 {
 468         /** Called by the kernel with a dump of the successful scan's data. Called for each SSID. */
 469         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 470         char bssid[NETCONFIG_BSSID_LEN+1];
 471         char ssid[NETCONFIG_SSID_LEN+1] = {0, };
 472         wifi_security_type_e sec_type = WIFI_SECURITY_TYPE_NONE;
 473         wifi_encryption_type_e enc_type = WIFI_ENCRYPTION_TYPE_NONE;
 474         GSList *vsie = NULL;
 475         struct nlattr *tb[NL80211_ATTR_MAX + 1];
 476         struct nlattr *bss[NL80211_BSS_MAX + 1];
 477         struct nla_policy bss_policy[NL80211_BSS_MAX + 1] = {
 478                 [NL80211_BSS_FREQUENCY] = {.type = NLA_U32},
 479                 [NL80211_BSS_BSSID] = { },
 480                 [NL80211_BSS_INFORMATION_ELEMENTS] = { },
 481                 [NL80211_BSS_SIGNAL_MBM] = {.type = NLA_U32},
 482         };
 483
 484         /** Parse nl message and check error. */
 485         nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
 486         if (!tb[NL80211_ATTR_BSS]) {
 487                 DBG("BSS info not available");
 488                 return NL_SKIP;
 489         }
 490         if (nla_parse_nested(bss, NL80211_BSS_MAX, tb[NL80211_ATTR_BSS], bss_policy)) {
 491                 DBG("Failed to parse nested attributes");
 492                 return NL_SKIP;
 493         }
 494         if ((!bss[NL80211_BSS_BSSID]) || (!bss[NL80211_BSS_INFORMATION_ELEMENTS]))
 495                 return NL_SKIP;
 496
 497         /** Extract BSSID and AP info. */
 498         __netconfig_macaddress_str(bssid, nla_data(bss[NL80211_BSS_BSSID]));
 499         __netconfig_found_ap(nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]), nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS]), ssid);
 500         __netconfig_get_vsie(nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]), nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS]), &vsie);
 501         __netconfig_get_security(nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]),
 502                         nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS]), &sec_type, &enc_type);
 503
 504         if (sec_type == WIFI_SECURITY_TYPE_EAP) {
 505                 g_slist_free_full(vsie, g_free);
 506                 return NL_SKIP;
 507         }
 508         for (GSList *list = bss_info_list; list != NULL; list = list->next) {
 509                 struct bss_scan_info_t *bss_info = (struct bss_scan_info_t *)list->data;
 510                 if ((g_strcmp0(bss_info->ssid, ssid) == 0) && (bss_info->security_type == sec_type)
 511                                 && (bss_info->encryption_type == enc_type)) {
 512                         g_slist_free_full(vsie, g_free);
 513                         return NL_SKIP;
 514                 }
 515         }
 516
 517         /** Create AP info list. */
 518         if (ssid[0] != '\0') {
 519                 struct bss_scan_info_t *bss_info;
 520                 int signal;
 521
 522                 bss_info = g_try_new0(struct bss_scan_info_t, 1);
 523                 if (bss_info == NULL) {
 524                         g_slist_free_full(vsie, g_free);
 525                         return NL_SKIP;
 526                 }
 527
 528                 g_strlcpy(bss_info->bssid, bssid, strlen(bssid)+1);
 529                 g_strlcpy(bss_info->ssid, ssid, strlen(ssid)+1);
 530                 bss_info->vsie_list = vsie;
 531                 bss_info->freq = nla_get_u32(bss[NL80211_BSS_FREQUENCY]);
 532
 533                 if (bss[NL80211_BSS_SIGNAL_MBM]) {
 534                         signal = nla_get_u32(bss[NL80211_BSS_SIGNAL_MBM]);
 535                         signal /= 100; /** mBm to dBm */
 536                         bss_info->signal = signal;
 537                 }
 538
 539                 bss_info->security_type = sec_type;
 540                 bss_info->encryption_type = enc_type;
 541                 DBG("%s %d %d %s %d %d ", bss_info->bssid, bss_info->freq,
 542                                 bss_info->signal, bss_info->ssid, bss_info->security_type,
 543                                 bss_info->encryption_type);
 544
 545                 if (bss_info->ssid[0] == '\0')
 546                         g_free(bss_info);
 547                 else
 548                         bss_info_list = g_slist_append(bss_info_list, bss_info);
 549
 550         } else
 551                 g_slist_free_full(vsie, g_free);
 552
 553         return NL_SKIP;
 554 }
 555
 556 static int __netconfig_netlink_scan_reply(struct nl_msg *msg, void *user_data)
 557 {
 558         /** Called by the kernel when the scan is done or has been aborted. */
 559         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 560         struct netconfig_netlink_scan_results *results = user_data;
 561
 562         if (gnlh->cmd == NL80211_CMD_NEW_SCAN_RESULTS) {
 563                 DBG("Received NL80211_CMD_NEW_SCAN_RESULTS in reply");
 564                 results->done = 1;
 565                 results->aborted = 0;
 566         } else if (gnlh->cmd == NL80211_CMD_SCAN_ABORTED) {
 567                 DBG("Received NL80211_CMD_SCAN_ABORTED in reply");
 568                 results->done = 1;
 569                 results->aborted = 1;
 570         }
 571
 572         return NL_SKIP;
 573 }
 574
 575 static int __netconfig_request_netlink_scan(struct nl_sock *socket,
 576                 int if_index, int id, GVariant *params)
 577 {
 578         struct netconfig_netlink_scan_results results = { .done = 0, .aborted = 0 };
 579         struct nl_msg *msg = NULL;
 580         struct nl_cb *cb = NULL;
 581         struct nl_msg *ssids = NULL;
 582         int err = 0;
 583         int ret = 0;
 584         unsigned char ies[NETCONFIG_MAX_VSIE_LEN+1] = {0x00, };
 585         int ies_len = 0;
 586         GVariantIter *iter;
 587         GVariant *value;
 588         gchar *key;
 589         gboolean ssid_found = FALSE;
 590         int mcid = __netconfig_get_multicast_id(socket, "nl80211", "scan");
 591
 592         ret = nl_socket_add_membership(socket, mcid);
 593         if (ret < 0) {
 594                 DBG("Failed to add membership, error: (%s)", nl_geterror(-ret));
 595                 return ret;
 596         }
 597
 598         msg = nlmsg_alloc();
 599         if (!msg) {
 600                 DBG("Failed to allocate msg");
 601                 nl_socket_drop_membership(socket, mcid);
 602                 return -ENOMEM;
 603         }
 604         ssids = nlmsg_alloc();
 605         if (!ssids) {
 606                 DBG("Failed to allocate ssids");
 607                 nlmsg_free(msg);
 608                 nl_socket_drop_membership(socket, mcid);
 609                 return -ENOMEM;
 610         }
 611         cb = nl_cb_alloc(NL_CB_DEFAULT);
 612         if (!cb) {
 613                 DBG("Failed to allocate callbacks");
 614                 nlmsg_free(msg);
 615                 nlmsg_free(ssids);
 616                 nl_socket_drop_membership(socket, mcid);
 617                 return -ENOMEM;
 618         }
 619
 620         /** Set nl message and callback functions. */
 621         genlmsg_put(msg, 0, 0, id, 0, 0, NL80211_CMD_TRIGGER_SCAN, 0);
 622         ret = nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index);
 623         if (ret < 0) {
 624                 DBG("Failed to add integer attribute to netlink message, error: (%s)", nl_geterror(-ret));
 625                 goto out;
 626         }
 627
 628         g_variant_get(params, "a{sv}", &iter);
 629         while (g_variant_iter_loop(iter, "{sv}", &key, &value)) {
 630                 if (g_strcmp0(key, "SSID") == 0) {
 631                         if (g_variant_is_of_type(value, G_VARIANT_TYPE_STRING)) {
 632                                 char *ssid = g_strdup(g_variant_get_string(value, NULL));
 633                                 ssid_found = TRUE;
 634                                 DBG("ssid [%s]", ssid);
 635
 636                                 ret = nla_put(ssids, 1, strlen(ssid), ssid);
 637                                 g_free(ssid);
 638                                 if (ret < 0) {
 639                                         DBG("Failed to add ssid to netlink message, error: (%s)", nl_geterror(-ret));
 640                                         g_variant_iter_free(iter);
 641                                         goto out;
 642                                 }
 643                         }
 644                 } else if (g_strcmp0(key, "VSIE") == 0) {
 645                         if (g_variant_is_of_type(value, G_VARIANT_TYPE_STRING)) {
 646                                 char *vsie = g_strdup(g_variant_get_string(value, NULL));
 647                                 int vsie_len = strlen(vsie);
 648                                 DBG("vsie: %s vsie_len: %d", vsie, vsie_len);
 649
 650                                 ies_len = (vsie_len % 2) ? ((vsie_len / 2) + 1) : (vsie_len / 2);
 651                                 __netconfig_hex_str_to_bin(vsie, ies, ies_len);
 652                                 g_free(vsie);
 653                         }
 654                 }
 655         }
 656         g_variant_iter_free(iter);
 657
 658         if (!ssid_found) {
 659                 ret = nla_put(ssids, 1, 0, "");
 660                 if (ret < 0) {
 661                         DBG("nla_put error: (%s)", nl_geterror(-ret));
 662                         goto out;
 663                 }
 664         }
 665         nla_put_nested(msg, NL80211_ATTR_SCAN_SSIDS, ssids);
 666
 667         if (ies[0] == NETCONFIG_VENDOR_SPECIFIC_ID && ies[1] >= 4) {
 668                 DBG("ies_len: %d ies: %02x %02x %02x %02x %02x %02x %02x", ies_len,
 669                                 ies[0], ies[1], ies[2], ies[3], ies[4], ies[5], ies[6]);
 670                 ret = nla_put(msg, NL80211_ATTR_IE, ies_len, ies);
 671                 if (ret < 0) {
 672                         DBG("Failed to add vsie data to netlink message, error: (%s)", nl_geterror(-ret));
 673                         goto out;
 674                 }
 675         }
 676
 677         err = 1;
 678         nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, __netconfig_netlink_scan_reply, &results);
 679         nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
 680         nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
 681         nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
 682         nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
 683
 684         /** Send NL80211_CMD_TRIGGER_SCAN to start the scan. */
 685         ret = nl_send_auto_complete(socket, msg);
 686         if (ret < 0) {
 687                 DBG("nl_send_auto_complete() error: (%s)", nl_geterror(-ret));
 688                 goto out;
 689         }
 690
 691         DBG("Sent %d bytes to the kernel", ret);
 692         ssid_found = FALSE;
 693
 694         while (err > 0)
 695                 ret = nl_recvmsgs(socket, cb);
 696
 697         if (ret < 0) {
 698                 DBG("nl_recvmsgs() ret: %d (%s)", ret, nl_geterror(-ret));
 699                 goto out;
 700         }
 701
 702         while (!results.done)
 703                 nl_recvmsgs(socket, cb);
 704
 705         if (results.aborted) {
 706                 DBG("scan aborted");
 707                 goto out;
 708         }
 709
 710 out:
 711         /** Release memory */
 712         nlmsg_free(ssids);
 713         nlmsg_free(msg);
 714         nl_cb_put(cb);
 715         nl_socket_drop_membership(socket, mcid);
 716
 717         if (ret < 0 || ret == 1)
 718                 return ret;
 719
 720         DBG("Scan done");
 721         return 0;
 722 }
 723
 724 static int __netconfig_initialize_nl80211(netconfig_nl_global *global)
 725 {
 726         int err = 0;
 727
 728         global->if_index = __netconfig_get_interface_index(WIFI_IFNAME);
 729         if (global->if_index < 0) {
 730                 DBG("Failed to get interface index");
 731                 return -1;
 732         }
 733
 734         global->socket = nl_socket_alloc();
 735         if (!global->socket) {
 736                 DBG("Failed to allocate netlink socket.");
 737                 return -ENOMEM;
 738         }
 739
 740         if (genl_connect(global->socket)) {
 741                 DBG("Failed to connect to generic netlink.");
 742                 err = -ENOLINK;
 743                 goto fail;
 744         }
 745
 746         global->id = genl_ctrl_resolve(global->socket, "nl80211");
 747         if (global->id < 0) {
 748                 DBG("Failed to find the nl80211 driver");
 749                 err = -ENOENT;
 750                 goto fail;
 751         }
 752
 753         return 0;
 754
 755 fail:
 756         nl_socket_free(global->socket);
 757         return err;
 758 }
 759
 760 static int __netconfig_initialize_nl_msg(netconfig_nl_global *global)
 761 {
 762         int rv;
 763
 764         if (global == NULL) {
 765                 DBG("Invalid parameter.");
 766                 return -EINVAL;
 767         }
 768
 769         global->msg = nlmsg_alloc();
 770         if (global->msg == NULL) {
 771                 DBG("Failed to allocate netlink message");
 772                 return -ENOMEM;
 773         }
 774
 775         /* Set command into message */
 776         genlmsg_put(global->msg, 0, 0, global->id, 0, NLM_F_DUMP, NL80211_CMD_GET_SCAN, 0);
 777         rv = nla_put_u32(global->msg, NL80211_ATTR_IFINDEX, global->if_index);
 778         if (rv < 0) {
 779                 DBG("Failed to add integer attribute to netlink message.");
 780                 nlmsg_free(global->msg);
 781                 return rv;
 782         }
 783         nl_socket_modify_cb(global->socket, NL_CB_VALID, NL_CB_CUSTOM, __netconfig_netlink_scan_cb, NULL);
 784
 785         return 0;
 786 }
 787
 788 int handle_netlink_scan(Wifi *wifi, GDBusMethodInvocation *context, GVariant *params)
 789 {
 790         DBG("");
 791         netconfig_nl_global global = {
 792                 .id = -1,
 793                 .if_index = -1,
 794                 .socket = NULL,
 795                 .msg = NULL,
 796         };
 797
 798         /** Initialize netlink socket */
 799         int ret = __netconfig_initialize_nl80211(&global);
 800         if (ret < 0) {
 801                 DBG("__netconfig_initialize_nl80211() failed, error %d", ret);
 802                 wifi_complete_netlink_scan(wifi, context);
 803                 return TRUE;
 804         }
 805
 806         /** Request NL80211_CMD_TRIGGER_SCAN to the kernel. */
 807         ret = __netconfig_request_netlink_scan(global.socket, global.if_index, global.id, params);
 808         if (ret < 0) {
 809                 DBG("__netconfig_request_netlink_scan() failed, error %d", ret);
 810                 wifi_complete_netlink_scan(wifi, context);
 811                 return TRUE;
 812         }
 813
 814         ret = __netconfig_initialize_nl_msg(&global);
 815         if (ret < 0) {
 816                 DBG("__netconfig_initialize_nl_msg() failed, error %d", ret);
 817                 wifi_complete_netlink_scan(wifi, context);
 818                 return TRUE;
 819         }
 820
 821         ret = nl_send_auto_complete(global.socket, global.msg);
 822         DBG("NL80211_CMD_GET_SCAN sent %d bytes to the kernel", ret);
 823
 824         /** Receive the kernel message. */
 825         ret = nl_recvmsgs_default(global.socket);
 826         nlmsg_free(global.msg);
 827         if (ret < 0) {
 828                 DBG("nl_recvmsgs_default() failed. ret: %d (error: %s)", ret, nl_geterror(-ret));
 829                 wifi_complete_netlink_scan(wifi, context);
 830                 return TRUE;
 831         }
 832
 833         wifi_complete_netlink_scan(wifi, context);
 834         __netconfig_notify_netlink_scan_done();
 835
 836         return TRUE;
 837 }