mesh: Write close reason from Mesh Peering Close to debug log
[mech_eap.git] / wpa_supplicant / interworking.c
index 19b6e38..589ee57 100644 (file)
@@ -73,17 +73,23 @@ static int cred_prio_cmp(const struct wpa_cred *a, const struct wpa_cred *b)
 
 static void interworking_reconnect(struct wpa_supplicant *wpa_s)
 {
+       unsigned int tried;
+
        if (wpa_s->wpa_state >= WPA_AUTHENTICATING) {
                wpa_supplicant_cancel_sched_scan(wpa_s);
+               wpa_s->own_disconnect_req = 1;
                wpa_supplicant_deauthenticate(wpa_s,
                                              WLAN_REASON_DEAUTH_LEAVING);
        }
        wpa_s->disconnected = 0;
        wpa_s->reassociate = 1;
+       tried = wpa_s->interworking_fast_assoc_tried;
+       wpa_s->interworking_fast_assoc_tried = 1;
 
-       if (wpa_supplicant_fast_associate(wpa_s) >= 0)
+       if (!tried && wpa_supplicant_fast_associate(wpa_s) >= 0)
                return;
 
+       wpa_s->interworking_fast_assoc_tried = 0;
        wpa_supplicant_req_scan(wpa_s, 0, 0);
 }
 
@@ -245,8 +251,8 @@ static int interworking_anqp_send_req(struct wpa_supplicant *wpa_s,
        struct wpabuf *extra = NULL;
        int all = wpa_s->fetch_all_anqp;
 
-       wpa_printf(MSG_DEBUG, "Interworking: ANQP Query Request to " MACSTR,
-                  MAC2STR(bss->bssid));
+       wpa_msg(wpa_s, MSG_DEBUG, "Interworking: ANQP Query Request to " MACSTR,
+               MAC2STR(bss->bssid));
        wpa_s->interworking_gas_bss = bss;
 
        info_ids[num_info_ids++] = ANQP_CAPABILITY_LIST;
@@ -307,14 +313,14 @@ static int interworking_anqp_send_req(struct wpa_supplicant *wpa_s,
        res = gas_query_req(wpa_s->gas, bss->bssid, bss->freq, buf,
                            interworking_anqp_resp_cb, wpa_s);
        if (res < 0) {
-               wpa_printf(MSG_DEBUG, "ANQP: Failed to send Query Request");
+               wpa_msg(wpa_s, MSG_DEBUG, "ANQP: Failed to send Query Request");
                wpabuf_free(buf);
                ret = -1;
                eloop_register_timeout(0, 0, interworking_continue_anqp, wpa_s,
                                       NULL);
        } else
-               wpa_printf(MSG_DEBUG, "ANQP: Query started with dialog token "
-                          "%u", res);
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "ANQP: Query started with dialog token %u", res);
 
        return ret;
 }
@@ -356,13 +362,13 @@ static const u8 * nai_realm_parse_eap(struct nai_realm_eap *e, const u8 *pos,
        u8 elen, auth_count, a;
        const u8 *e_end;
 
-       if (pos + 3 > end) {
+       if (end - pos < 3) {
                wpa_printf(MSG_DEBUG, "No room for EAP Method fixed fields");
                return NULL;
        }
 
        elen = *pos++;
-       if (pos + elen > end || elen < 2) {
+       if (elen > end - pos || elen < 2) {
                wpa_printf(MSG_DEBUG, "No room for EAP Method subfield");
                return NULL;
        }
@@ -375,14 +381,19 @@ static const u8 * nai_realm_parse_eap(struct nai_realm_eap *e, const u8 *pos,
        for (a = 0; a < auth_count; a++) {
                u8 id, len;
 
-               if (pos + 2 > end || pos + 2 + pos[1] > end) {
-                       wpa_printf(MSG_DEBUG, "No room for Authentication "
-                                  "Parameter subfield");
+               if (end - pos < 2) {
+                       wpa_printf(MSG_DEBUG,
+                                  "No room for Authentication Parameter subfield header");
                        return NULL;
                }
 
                id = *pos++;
                len = *pos++;
+               if (len > end - pos) {
+                       wpa_printf(MSG_DEBUG,
+                                  "No room for Authentication Parameter subfield");
+                       return NULL;
+               }
 
                switch (id) {
                case NAI_REALM_EAP_AUTH_NON_EAP_INNER_AUTH:
@@ -457,7 +468,7 @@ static const u8 * nai_realm_parse_realm(struct nai_realm *r, const u8 *pos,
 
        len = WPA_GET_LE16(pos); /* NAI Realm Data field Length */
        pos += 2;
-       if (pos + len > end || len < 3) {
+       if (len > end - pos || len < 3) {
                wpa_printf(MSG_DEBUG, "No room for NAI Realm Data "
                           "(len=%u; left=%u)",
                           len, (unsigned int) (end - pos));
@@ -467,7 +478,7 @@ static const u8 * nai_realm_parse_realm(struct nai_realm *r, const u8 *pos,
 
        r->encoding = *pos++;
        realm_len = *pos++;
-       if (pos + realm_len > f_end) {
+       if (realm_len > f_end - pos) {
                wpa_printf(MSG_DEBUG, "No room for NAI Realm "
                           "(len=%u; left=%u)",
                           realm_len, (unsigned int) (f_end - pos));
@@ -479,13 +490,13 @@ static const u8 * nai_realm_parse_realm(struct nai_realm *r, const u8 *pos,
                return NULL;
        pos += realm_len;
 
-       if (pos + 1 > f_end) {
+       if (f_end - pos < 1) {
                wpa_printf(MSG_DEBUG, "No room for EAP Method Count");
                return NULL;
        }
        r->eap_count = *pos++;
        wpa_printf(MSG_DEBUG, "EAP Count: %u", r->eap_count);
-       if (pos + r->eap_count * 3 > f_end) {
+       if (r->eap_count * 3 > f_end - pos) {
                wpa_printf(MSG_DEBUG, "No room for EAP Methods");
                return NULL;
        }
@@ -508,20 +519,25 @@ static struct nai_realm * nai_realm_parse(struct wpabuf *anqp, u16 *count)
        struct nai_realm *realm;
        const u8 *pos, *end;
        u16 i, num;
+       size_t left;
 
-       if (anqp == NULL || wpabuf_len(anqp) < 2)
+       if (anqp == NULL)
+               return NULL;
+       left = wpabuf_len(anqp);
+       if (left < 2)
                return NULL;
 
        pos = wpabuf_head_u8(anqp);
-       end = pos + wpabuf_len(anqp);
+       end = pos + left;
        num = WPA_GET_LE16(pos);
        wpa_printf(MSG_DEBUG, "NAI Realm Count: %u", num);
        pos += 2;
+       left -= 2;
 
-       if (num * 5 > end - pos) {
+       if (num > left / 5) {
                wpa_printf(MSG_DEBUG, "Invalid NAI Realm Count %u - not "
                           "enough data (%u octets) for that many realms",
-                          num, (unsigned int) (end - pos));
+                          num, (unsigned int) left);
                return NULL;
        }
 
@@ -577,56 +593,91 @@ static int nai_realm_match(struct nai_realm *realm, const char *home_realm)
 }
 
 
-static int nai_realm_cred_username(struct nai_realm_eap *eap)
+static int nai_realm_cred_username(struct wpa_supplicant *wpa_s,
+                                  struct nai_realm_eap *eap)
 {
-       if (eap_get_name(EAP_VENDOR_IETF, eap->method) == NULL)
+       if (eap_get_name(EAP_VENDOR_IETF, eap->method) == NULL) {
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "nai-realm-cred-username: EAP method not supported: %d",
+                       eap->method);
                return 0; /* method not supported */
+       }
 
        if (eap->method != EAP_TYPE_TTLS && eap->method != EAP_TYPE_PEAP &&
            eap->method != EAP_TYPE_FAST) {
                /* Only tunneled methods with username/password supported */
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "nai-realm-cred-username: Method: %d is not TTLS, PEAP, or FAST",
+                       eap->method);
                return 0;
        }
 
        if (eap->method == EAP_TYPE_PEAP || eap->method == EAP_TYPE_FAST) {
                if (eap->inner_method &&
-                   eap_get_name(EAP_VENDOR_IETF, eap->inner_method) == NULL)
+                   eap_get_name(EAP_VENDOR_IETF, eap->inner_method) == NULL) {
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "nai-realm-cred-username: PEAP/FAST: Inner method not supported: %d",
+                               eap->inner_method);
                        return 0;
+               }
                if (!eap->inner_method &&
-                   eap_get_name(EAP_VENDOR_IETF, EAP_TYPE_MSCHAPV2) == NULL)
+                   eap_get_name(EAP_VENDOR_IETF, EAP_TYPE_MSCHAPV2) == NULL) {
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "nai-realm-cred-username: MSCHAPv2 not supported");
                        return 0;
+               }
        }
 
        if (eap->method == EAP_TYPE_TTLS) {
                if (eap->inner_method == 0 && eap->inner_non_eap == 0)
                        return 1; /* Assume TTLS/MSCHAPv2 is used */
                if (eap->inner_method &&
-                   eap_get_name(EAP_VENDOR_IETF, eap->inner_method) == NULL)
+                   eap_get_name(EAP_VENDOR_IETF, eap->inner_method) == NULL) {
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "nai-realm-cred-username: TTLS, but inner not supported: %d",
+                               eap->inner_method);
                        return 0;
+               }
                if (eap->inner_non_eap &&
                    eap->inner_non_eap != NAI_REALM_INNER_NON_EAP_PAP &&
                    eap->inner_non_eap != NAI_REALM_INNER_NON_EAP_CHAP &&
                    eap->inner_non_eap != NAI_REALM_INNER_NON_EAP_MSCHAP &&
-                   eap->inner_non_eap != NAI_REALM_INNER_NON_EAP_MSCHAPV2)
+                   eap->inner_non_eap != NAI_REALM_INNER_NON_EAP_MSCHAPV2) {
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "nai-realm-cred-username: TTLS, inner-non-eap not supported: %d",
+                               eap->inner_non_eap);
                        return 0;
+               }
        }
 
        if (eap->inner_method &&
            eap->inner_method != EAP_TYPE_GTC &&
-           eap->inner_method != EAP_TYPE_MSCHAPV2)
+           eap->inner_method != EAP_TYPE_MSCHAPV2) {
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "nai-realm-cred-username: inner-method not GTC or MSCHAPv2: %d",
+                       eap->inner_method);
                return 0;
+       }
 
        return 1;
 }
 
 
-static int nai_realm_cred_cert(struct nai_realm_eap *eap)
+static int nai_realm_cred_cert(struct wpa_supplicant *wpa_s,
+                              struct nai_realm_eap *eap)
 {
-       if (eap_get_name(EAP_VENDOR_IETF, eap->method) == NULL)
+       if (eap_get_name(EAP_VENDOR_IETF, eap->method) == NULL) {
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "nai-realm-cred-cert: Method not supported: %d",
+                       eap->method);
                return 0; /* method not supported */
+       }
 
        if (eap->method != EAP_TYPE_TLS) {
                /* Only EAP-TLS supported for credential authentication */
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "nai-realm-cred-cert: Method not TLS: %d",
+                       eap->method);
                return 0;
        }
 
@@ -634,27 +685,33 @@ static int nai_realm_cred_cert(struct nai_realm_eap *eap)
 }
 
 
-static struct nai_realm_eap * nai_realm_find_eap(struct wpa_cred *cred,
+static struct nai_realm_eap * nai_realm_find_eap(struct wpa_supplicant *wpa_s,
+                                                struct wpa_cred *cred,
                                                 struct nai_realm *realm)
 {
        u8 e;
 
-       if (cred == NULL ||
-           cred->username == NULL ||
+       if (cred->username == NULL ||
            cred->username[0] == '\0' ||
            ((cred->password == NULL ||
              cred->password[0] == '\0') &&
             (cred->private_key == NULL ||
-             cred->private_key[0] == '\0')))
+             cred->private_key[0] == '\0'))) {
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "nai-realm-find-eap: incomplete cred info: username: %s  password: %s private_key: %s",
+                       cred->username ? cred->username : "NULL",
+                       cred->password ? cred->password : "NULL",
+                       cred->private_key ? cred->private_key : "NULL");
                return NULL;
+       }
 
        for (e = 0; e < realm->eap_count; e++) {
                struct nai_realm_eap *eap = &realm->eap[e];
                if (cred->password && cred->password[0] &&
-                   nai_realm_cred_username(eap))
+                   nai_realm_cred_username(wpa_s, eap))
                        return eap;
                if (cred->private_key && cred->private_key[0] &&
-                   nai_realm_cred_cert(eap))
+                   nai_realm_cred_cert(wpa_s, eap))
                        return eap;
        }
 
@@ -694,7 +751,7 @@ static int plmn_id_match(struct wpabuf *anqp, const char *imsi, int mnc_len)
                return 0;
        pos = wpabuf_head_u8(anqp);
        end = pos + wpabuf_len(anqp);
-       if (pos + 2 > end)
+       if (end - pos < 2)
                return 0;
        if (*pos != 0) {
                wpa_printf(MSG_DEBUG, "Unsupported GUD version 0x%x", *pos);
@@ -702,7 +759,7 @@ static int plmn_id_match(struct wpabuf *anqp, const char *imsi, int mnc_len)
        }
        pos++;
        udhl = *pos++;
-       if (pos + udhl > end) {
+       if (udhl > end - pos) {
                wpa_printf(MSG_DEBUG, "Invalid UDHL");
                return 0;
        }
@@ -712,12 +769,12 @@ static int plmn_id_match(struct wpabuf *anqp, const char *imsi, int mnc_len)
                   plmn[0], plmn[1], plmn[2], plmn2[0], plmn2[1], plmn2[2],
                   imsi, mnc_len);
 
-       while (pos + 2 <= end) {
+       while (end - pos >= 2) {
                u8 iei, len;
                const u8 *l_end;
                iei = *pos++;
                len = *pos++ & 0x7f;
-               if (pos + len > end)
+               if (len > end - pos)
                        break;
                l_end = pos + len;
 
@@ -728,7 +785,7 @@ static int plmn_id_match(struct wpabuf *anqp, const char *imsi, int mnc_len)
                                    pos, len);
                        num = *pos++;
                        for (i = 0; i < num; i++) {
-                               if (pos + 3 > l_end)
+                               if (l_end - pos < 3)
                                        break;
                                if (os_memcmp(pos, plmn, 3) == 0 ||
                                    os_memcmp(pos, plmn2, 3) == 0)
@@ -864,6 +921,7 @@ static void remove_duplicate_network(struct wpa_supplicant *wpa_s,
        if (ssid == wpa_s->current_ssid) {
                wpa_sm_set_config(wpa_s->wpa, NULL);
                eapol_sm_notify_config(wpa_s->eapol, NULL, NULL);
+               wpa_s->own_disconnect_req = 1;
                wpa_supplicant_deauthenticate(wpa_s,
                                              WLAN_REASON_DEAUTH_LEAVING);
        }
@@ -904,7 +962,7 @@ static int interworking_set_hs20_params(struct wpa_supplicant *wpa_s,
 
 static int interworking_connect_3gpp(struct wpa_supplicant *wpa_s,
                                     struct wpa_cred *cred,
-                                    struct wpa_bss *bss)
+                                    struct wpa_bss *bss, int only_add)
 {
 #ifdef INTERWORKING_3GPP
        struct wpa_ssid *ssid;
@@ -915,13 +973,13 @@ static int interworking_connect_3gpp(struct wpa_supplicant *wpa_s,
        if (bss->anqp == NULL || bss->anqp->anqp_3gpp == NULL)
                return -1;
 
-       wpa_printf(MSG_DEBUG, "Interworking: Connect with " MACSTR " (3GPP)",
-                  MAC2STR(bss->bssid));
+       wpa_msg(wpa_s, MSG_DEBUG, "Interworking: Connect with " MACSTR
+               " (3GPP)", MAC2STR(bss->bssid));
 
        if (already_connected(wpa_s, cred, bss)) {
                wpa_msg(wpa_s, MSG_INFO, INTERWORKING_ALREADY_CONNECTED MACSTR,
                        MAC2STR(bss->bssid));
-               return 0;
+               return wpa_s->current_ssid->id;
        }
 
        remove_duplicate_network(wpa_s, cred, bss);
@@ -973,13 +1031,13 @@ static int interworking_connect_3gpp(struct wpa_supplicant *wpa_s,
                break;
        }
        if (res < 0) {
-               wpa_printf(MSG_DEBUG, "Selected EAP method (%d) not supported",
-                          eap_type);
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Selected EAP method (%d) not supported", eap_type);
                goto fail;
        }
 
        if (!cred->pcsc && set_root_nai(ssid, cred->imsi, prefix) < 0) {
-               wpa_printf(MSG_DEBUG, "Failed to set Root NAI");
+               wpa_msg(wpa_s, MSG_DEBUG, "Failed to set Root NAI");
                goto fail;
        }
 
@@ -998,9 +1056,10 @@ static int interworking_connect_3gpp(struct wpa_supplicant *wpa_s,
 
        wpa_s->next_ssid = ssid;
        wpa_config_update_prio_list(wpa_s->conf);
-       interworking_reconnect(wpa_s);
+       if (!only_add)
+               interworking_reconnect(wpa_s);
 
-       return 0;
+       return ssid->id;
 
 fail:
        wpas_notify_network_removed(wpa_s, ssid);
@@ -1028,12 +1087,12 @@ static int roaming_consortium_element_match(const u8 *ie, const u8 *rc_id,
         * OI #1, [OI #2], [OI #3]
         */
 
-       if (pos + 2 > end)
+       if (end - pos < 2)
                return 0;
 
        pos++; /* skip Number of ANQP OIs */
        lens = *pos++;
-       if (pos + (lens & 0x0f) + (lens >> 4) > end)
+       if ((lens & 0x0f) + (lens >> 4) > end - pos)
                return 0;
 
        if ((lens & 0x0f) == rc_len && os_memcmp(pos, rc_id, rc_len) == 0)
@@ -1067,7 +1126,7 @@ static int roaming_consortium_anqp_match(const struct wpabuf *anqp,
        /* Set of <OI Length, OI> duples */
        while (pos < end) {
                len = *pos++;
-               if (pos + len > end)
+               if (len > end - pos)
                        break;
                if (len == rc_len && os_memcmp(pos, rc_id, rc_len) == 0)
                        return 1;
@@ -1128,6 +1187,7 @@ static int cred_excluded_ssid(struct wpa_cred *cred, struct wpa_bss *bss)
 static int cred_below_min_backhaul(struct wpa_supplicant *wpa_s,
                                   struct wpa_cred *cred, struct wpa_bss *bss)
 {
+#ifdef CONFIG_HS20
        int res;
        unsigned int dl_bandwidth, ul_bandwidth;
        const u8 *wan;
@@ -1179,6 +1239,7 @@ static int cred_below_min_backhaul(struct wpa_supplicant *wpa_s,
                if (cred->min_ul_bandwidth_roaming > ul_bandwidth)
                        return 1;
        }
+#endif /* CONFIG_HS20 */
 
        return 0;
 }
@@ -1206,9 +1267,11 @@ static int cred_over_max_bss_load(struct wpa_supplicant *wpa_s,
 }
 
 
+#ifdef CONFIG_HS20
+
 static int has_proto_match(const u8 *pos, const u8 *end, u8 proto)
 {
-       while (pos + 4 <= end) {
+       while (end - pos >= 4) {
                if (pos[0] == proto && pos[3] == 1 /* Open */)
                        return 1;
                pos += 4;
@@ -1221,7 +1284,7 @@ static int has_proto_match(const u8 *pos, const u8 *end, u8 proto)
 static int has_proto_port_match(const u8 *pos, const u8 *end, u8 proto,
                                u16 port)
 {
-       while (pos + 4 <= end) {
+       while (end - pos >= 4) {
                if (pos[0] == proto && WPA_GET_LE16(&pos[1]) == port &&
                    pos[3] == 1 /* Open */)
                        return 1;
@@ -1231,10 +1294,13 @@ static int has_proto_port_match(const u8 *pos, const u8 *end, u8 proto,
        return 0;
 }
 
+#endif /* CONFIG_HS20 */
+
 
 static int cred_conn_capab_missing(struct wpa_supplicant *wpa_s,
                                   struct wpa_cred *cred, struct wpa_bss *bss)
 {
+#ifdef CONFIG_HS20
        int res;
        const u8 *capab, *end;
        unsigned int i, j;
@@ -1271,6 +1337,7 @@ static int cred_conn_capab_missing(struct wpa_supplicant *wpa_s,
                        }
                }
        }
+#endif /* CONFIG_HS20 */
 
        return 0;
 }
@@ -1384,7 +1451,24 @@ static int interworking_set_eap_params(struct wpa_ssid *ssid,
                os_free(anon);
        }
 
-       if (cred->username && cred->username[0] &&
+       if (!ttls && cred->username && cred->username[0] && cred->realm &&
+           !os_strchr(cred->username, '@')) {
+               char *id;
+               size_t buflen;
+               int res;
+
+               buflen = os_strlen(cred->username) + 1 +
+                       os_strlen(cred->realm) + 1;
+
+               id = os_malloc(buflen);
+               if (!id)
+                       return -1;
+               os_snprintf(id, buflen, "%s@%s", cred->username, cred->realm);
+               res = wpa_config_set_quoted(ssid, "identity", id);
+               os_free(id);
+               if (res < 0)
+                       return -1;
+       } else if (cred->username && cred->username[0] &&
            wpa_config_set_quoted(ssid, "identity", cred->username) < 0)
                return -1;
 
@@ -1448,17 +1532,17 @@ static int interworking_set_eap_params(struct wpa_ssid *ssid,
 
 static int interworking_connect_roaming_consortium(
        struct wpa_supplicant *wpa_s, struct wpa_cred *cred,
-       struct wpa_bss *bss)
+       struct wpa_bss *bss, int only_add)
 {
        struct wpa_ssid *ssid;
 
-       wpa_printf(MSG_DEBUG, "Interworking: Connect with " MACSTR " based on "
-                  "roaming consortium match", MAC2STR(bss->bssid));
+       wpa_msg(wpa_s, MSG_DEBUG, "Interworking: Connect with " MACSTR
+               " based on roaming consortium match", MAC2STR(bss->bssid));
 
        if (already_connected(wpa_s, cred, bss)) {
                wpa_msg(wpa_s, MSG_INFO, INTERWORKING_ALREADY_CONNECTED MACSTR,
                        MAC2STR(bss->bssid));
-               return 0;
+               return wpa_s->current_ssid->id;
        }
 
        remove_duplicate_network(wpa_s, cred, bss);
@@ -1481,8 +1565,8 @@ static int interworking_connect_roaming_consortium(
                goto fail;
 
        if (cred->eap_method == NULL) {
-               wpa_printf(MSG_DEBUG, "Interworking: No EAP method set for "
-                          "credential using roaming consortium");
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: No EAP method set for credential using roaming consortium");
                goto fail;
        }
 
@@ -1494,9 +1578,10 @@ static int interworking_connect_roaming_consortium(
 
        wpa_s->next_ssid = ssid;
        wpa_config_update_prio_list(wpa_s->conf);
-       interworking_reconnect(wpa_s);
+       if (!only_add)
+               interworking_reconnect(wpa_s);
 
-       return 0;
+       return ssid->id;
 
 fail:
        wpas_notify_network_removed(wpa_s, ssid);
@@ -1506,7 +1591,8 @@ fail:
 
 
 static int interworking_connect_helper(struct wpa_supplicant *wpa_s,
-                                      struct wpa_bss *bss, int allow_excluded)
+                                      struct wpa_bss *bss, int allow_excluded,
+                                      int only_add)
 {
        struct wpa_cred *cred, *cred_rc, *cred_3gpp;
        struct wpa_ssid *ssid;
@@ -1521,8 +1607,9 @@ static int interworking_connect_helper(struct wpa_supplicant *wpa_s,
                return -1;
        if (disallowed_bssid(wpa_s, bss->bssid) ||
            disallowed_ssid(wpa_s, bss->ssid, bss->ssid_len)) {
-               wpa_printf(MSG_DEBUG, "Interworking: Reject connection to disallowed BSS "
-                          MACSTR, MAC2STR(bss->bssid));
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Reject connection to disallowed BSS "
+                       MACSTR, MAC2STR(bss->bssid));
                return -1;
        }
 
@@ -1535,27 +1622,26 @@ static int interworking_connect_helper(struct wpa_supplicant *wpa_s,
                 * We currently support only HS 2.0 networks and those are
                 * required to use WPA2-Enterprise.
                 */
-               wpa_printf(MSG_DEBUG, "Interworking: Network does not use "
-                          "RSN");
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Network does not use RSN");
                return -1;
        }
 
        cred_rc = interworking_credentials_available_roaming_consortium(
                wpa_s, bss, 0, excl);
        if (cred_rc) {
-               wpa_printf(MSG_DEBUG, "Interworking: Highest roaming "
-                          "consortium matching credential priority %d "
-                          "sp_priority %d",
-                          cred_rc->priority, cred_rc->sp_priority);
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Highest roaming consortium matching credential priority %d sp_priority %d",
+                       cred_rc->priority, cred_rc->sp_priority);
                if (allow_excluded && excl && !(*excl))
                        excl = NULL;
        }
 
        cred = interworking_credentials_available_realm(wpa_s, bss, 0, excl);
        if (cred) {
-               wpa_printf(MSG_DEBUG, "Interworking: Highest NAI Realm list "
-                          "matching credential priority %d sp_priority %d",
-                          cred->priority, cred->sp_priority);
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Highest NAI Realm list matching credential priority %d sp_priority %d",
+                       cred->priority, cred->sp_priority);
                if (allow_excluded && excl && !(*excl))
                        excl = NULL;
        }
@@ -1563,22 +1649,22 @@ static int interworking_connect_helper(struct wpa_supplicant *wpa_s,
        cred_3gpp = interworking_credentials_available_3gpp(wpa_s, bss, 0,
                                                            excl);
        if (cred_3gpp) {
-               wpa_printf(MSG_DEBUG, "Interworking: Highest 3GPP matching "
-                          "credential priority %d sp_priority %d",
-                          cred_3gpp->priority, cred_3gpp->sp_priority);
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Highest 3GPP matching credential priority %d sp_priority %d",
+                       cred_3gpp->priority, cred_3gpp->sp_priority);
                if (allow_excluded && excl && !(*excl))
                        excl = NULL;
        }
 
        if (!cred_rc && !cred && !cred_3gpp) {
-               wpa_printf(MSG_DEBUG, "Interworking: No full credential matches - consider options without BW(etc.) limits");
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: No full credential matches - consider options without BW(etc.) limits");
                cred_rc = interworking_credentials_available_roaming_consortium(
                        wpa_s, bss, 1, excl);
                if (cred_rc) {
-                       wpa_printf(MSG_DEBUG, "Interworking: Highest roaming "
-                                  "consortium matching credential priority %d "
-                                  "sp_priority %d (ignore BW)",
-                                  cred_rc->priority, cred_rc->sp_priority);
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "Interworking: Highest roaming consortium matching credential priority %d sp_priority %d (ignore BW)",
+                               cred_rc->priority, cred_rc->sp_priority);
                        if (allow_excluded && excl && !(*excl))
                                excl = NULL;
                }
@@ -1586,10 +1672,9 @@ static int interworking_connect_helper(struct wpa_supplicant *wpa_s,
                cred = interworking_credentials_available_realm(wpa_s, bss, 1,
                                                                excl);
                if (cred) {
-                       wpa_printf(MSG_DEBUG, "Interworking: Highest NAI Realm "
-                                  "list matching credential priority %d "
-                                  "sp_priority %d (ignore BW)",
-                                  cred->priority, cred->sp_priority);
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "Interworking: Highest NAI Realm list matching credential priority %d sp_priority %d (ignore BW)",
+                               cred->priority, cred->sp_priority);
                        if (allow_excluded && excl && !(*excl))
                                excl = NULL;
                }
@@ -1597,10 +1682,9 @@ static int interworking_connect_helper(struct wpa_supplicant *wpa_s,
                cred_3gpp = interworking_credentials_available_3gpp(wpa_s, bss,
                                                                    1, excl);
                if (cred_3gpp) {
-                       wpa_printf(MSG_DEBUG, "Interworking: Highest 3GPP "
-                                  "matching credential priority %d "
-                                  "sp_priority %d (ignore BW)",
-                                  cred_3gpp->priority, cred_3gpp->sp_priority);
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "Interworking: Highest 3GPP matching credential priority %d sp_priority %d (ignore BW)",
+                               cred_3gpp->priority, cred_3gpp->sp_priority);
                        if (allow_excluded && excl && !(*excl))
                                excl = NULL;
                }
@@ -1610,45 +1694,48 @@ static int interworking_connect_helper(struct wpa_supplicant *wpa_s,
            (cred == NULL || cred_prio_cmp(cred_rc, cred) >= 0) &&
            (cred_3gpp == NULL || cred_prio_cmp(cred_rc, cred_3gpp) >= 0))
                return interworking_connect_roaming_consortium(wpa_s, cred_rc,
-                                                              bss);
+                                                              bss, only_add);
 
        if (cred_3gpp &&
            (cred == NULL || cred_prio_cmp(cred_3gpp, cred) >= 0)) {
-               return interworking_connect_3gpp(wpa_s, cred_3gpp, bss);
+               return interworking_connect_3gpp(wpa_s, cred_3gpp, bss,
+                                                only_add);
        }
 
        if (cred == NULL) {
-               wpa_printf(MSG_DEBUG, "Interworking: No matching credentials "
-                          "found for " MACSTR, MAC2STR(bss->bssid));
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: No matching credentials found for "
+                       MACSTR, MAC2STR(bss->bssid));
                return -1;
        }
 
        realm = nai_realm_parse(bss->anqp ? bss->anqp->nai_realm : NULL,
                                &count);
        if (realm == NULL) {
-               wpa_printf(MSG_DEBUG, "Interworking: Could not parse NAI "
-                          "Realm list from " MACSTR, MAC2STR(bss->bssid));
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Could not parse NAI Realm list from "
+                       MACSTR, MAC2STR(bss->bssid));
                return -1;
        }
 
        for (i = 0; i < count; i++) {
                if (!nai_realm_match(&realm[i], cred->realm))
                        continue;
-               eap = nai_realm_find_eap(cred, &realm[i]);
+               eap = nai_realm_find_eap(wpa_s, cred, &realm[i]);
                if (eap)
                        break;
        }
 
        if (!eap) {
-               wpa_printf(MSG_DEBUG, "Interworking: No matching credentials "
-                          "and EAP method found for " MACSTR,
-                          MAC2STR(bss->bssid));
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: No matching credentials and EAP method found for "
+                       MACSTR, MAC2STR(bss->bssid));
                nai_realm_free(realm, count);
                return -1;
        }
 
-       wpa_printf(MSG_DEBUG, "Interworking: Connect with " MACSTR,
-                  MAC2STR(bss->bssid));
+       wpa_msg(wpa_s, MSG_DEBUG, "Interworking: Connect with " MACSTR,
+               MAC2STR(bss->bssid));
 
        if (already_connected(wpa_s, cred, bss)) {
                wpa_msg(wpa_s, MSG_INFO, INTERWORKING_ALREADY_CONNECTED MACSTR,
@@ -1750,9 +1837,10 @@ static int interworking_connect_helper(struct wpa_supplicant *wpa_s,
 
        wpa_s->next_ssid = ssid;
        wpa_config_update_prio_list(wpa_s->conf);
-       interworking_reconnect(wpa_s);
+       if (!only_add)
+               interworking_reconnect(wpa_s);
 
-       return 0;
+       return ssid->id;
 
 fail:
        wpas_notify_network_removed(wpa_s, ssid);
@@ -1762,9 +1850,10 @@ fail:
 }
 
 
-int interworking_connect(struct wpa_supplicant *wpa_s, struct wpa_bss *bss)
+int interworking_connect(struct wpa_supplicant *wpa_s, struct wpa_bss *bss,
+                        int only_add)
 {
-       return interworking_connect_helper(wpa_s, bss, 1);
+       return interworking_connect_helper(wpa_s, bss, 1, only_add);
 }
 
 
@@ -1803,22 +1892,29 @@ static struct wpa_cred * interworking_credentials_available_3gpp(
        int ret;
        int is_excluded = 0;
 
-       if (bss->anqp == NULL || bss->anqp->anqp_3gpp == NULL)
+       if (bss->anqp == NULL || bss->anqp->anqp_3gpp == NULL) {
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "interworking-avail-3gpp: not avail, anqp: %p  anqp_3gpp: %p",
+                       bss->anqp, bss->anqp ? bss->anqp->anqp_3gpp : NULL);
                return NULL;
+       }
 
 #ifdef CONFIG_EAP_PROXY
        if (!wpa_s->imsi[0]) {
                size_t len;
-               wpa_printf(MSG_DEBUG, "Interworking: IMSI not available - try to read again through eap_proxy");
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: IMSI not available - try to read again through eap_proxy");
                wpa_s->mnc_len = eapol_sm_get_eap_proxy_imsi(wpa_s->eapol,
                                                             wpa_s->imsi,
                                                             &len);
                if (wpa_s->mnc_len > 0) {
                        wpa_s->imsi[len] = '\0';
-                       wpa_printf(MSG_DEBUG, "eap_proxy: IMSI %s (MNC length %d)",
-                                  wpa_s->imsi, wpa_s->mnc_len);
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "eap_proxy: IMSI %s (MNC length %d)",
+                               wpa_s->imsi, wpa_s->mnc_len);
                } else {
-                       wpa_printf(MSG_DEBUG, "eap_proxy: IMSI not available");
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "eap_proxy: IMSI not available");
                }
        }
 #endif /* CONFIG_EAP_PROXY */
@@ -1869,10 +1965,12 @@ static struct wpa_cred * interworking_credentials_available_3gpp(
 #if defined(PCSC_FUNCS) || defined(CONFIG_EAP_PROXY)
        compare:
 #endif /* PCSC_FUNCS || CONFIG_EAP_PROXY */
-               wpa_printf(MSG_DEBUG, "Interworking: Parsing 3GPP info from "
-                          MACSTR, MAC2STR(bss->bssid));
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Parsing 3GPP info from " MACSTR,
+                       MAC2STR(bss->bssid));
                ret = plmn_id_match(bss->anqp->anqp_3gpp, imsi, mnc_len);
-               wpa_printf(MSG_DEBUG, "PLMN match %sfound", ret ? "" : "not ");
+               wpa_msg(wpa_s, MSG_DEBUG, "PLMN match %sfound",
+                       ret ? "" : "not ");
                if (ret) {
                        if (cred_no_required_oi_match(cred, bss))
                                continue;
@@ -1924,12 +2022,13 @@ static struct wpa_cred * interworking_credentials_available_realm(
        if (wpa_s->conf->cred == NULL)
                return NULL;
 
-       wpa_printf(MSG_DEBUG, "Interworking: Parsing NAI Realm list from "
-                  MACSTR, MAC2STR(bss->bssid));
+       wpa_msg(wpa_s, MSG_DEBUG, "Interworking: Parsing NAI Realm list from "
+               MACSTR, MAC2STR(bss->bssid));
        realm = nai_realm_parse(bss->anqp->nai_realm, &count);
        if (realm == NULL) {
-               wpa_printf(MSG_DEBUG, "Interworking: Could not parse NAI "
-                          "Realm list from " MACSTR, MAC2STR(bss->bssid));
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Could not parse NAI Realm list from "
+                       MACSTR, MAC2STR(bss->bssid));
                return NULL;
        }
 
@@ -1940,7 +2039,7 @@ static struct wpa_cred * interworking_credentials_available_realm(
                for (i = 0; i < count; i++) {
                        if (!nai_realm_match(&realm[i], cred->realm))
                                continue;
-                       if (nai_realm_find_eap(cred, &realm[i])) {
+                       if (nai_realm_find_eap(wpa_s, cred, &realm[i])) {
                                if (cred_no_required_oi_match(cred, bss))
                                        continue;
                                if (!ignore_bw &&
@@ -1968,6 +2067,9 @@ static struct wpa_cred * interworking_credentials_available_realm(
                                        }
                                }
                                break;
+                       } else {
+                               wpa_msg(wpa_s, MSG_DEBUG,
+                                       "Interworking: realm-find-eap returned false");
                        }
                }
        }
@@ -1986,7 +2088,7 @@ static struct wpa_cred * interworking_credentials_available_helper(
        int *excluded)
 {
        struct wpa_cred *cred, *cred2;
-       int excluded1, excluded2;
+       int excluded1, excluded2 = 0;
 
        if (disallowed_bssid(wpa_s, bss->bssid) ||
            disallowed_ssid(wpa_s, bss->ssid, bss->ssid_len)) {
@@ -2053,23 +2155,27 @@ int domain_name_list_contains(struct wpabuf *domain_names,
        pos = wpabuf_head(domain_names);
        end = pos + wpabuf_len(domain_names);
 
-       while (pos + 1 < end) {
-               if (pos + 1 + pos[0] > end)
+       while (end - pos > 1) {
+               u8 elen;
+
+               elen = *pos++;
+               if (elen > end - pos)
                        break;
 
                wpa_hexdump_ascii(MSG_DEBUG, "Interworking: AP domain name",
-                                 pos + 1, pos[0]);
-               if (pos[0] == len &&
-                   os_strncasecmp(domain, (const char *) (pos + 1), len) == 0)
+                                 pos, elen);
+               if (elen == len &&
+                   os_strncasecmp(domain, (const char *) pos, len) == 0)
                        return 1;
-               if (!exact_match && pos[0] > len && pos[pos[0] - len] == '.') {
-                       const char *ap = (const char *) (pos + 1);
-                       int offset = pos[0] - len;
+               if (!exact_match && elen > len && pos[elen - len - 1] == '.') {
+                       const char *ap = (const char *) pos;
+                       int offset = elen - len;
+
                        if (os_strncasecmp(domain, ap + offset, len) == 0)
                                return 1;
                }
 
-               pos += 1 + pos[0];
+               pos += elen;
        }
 
        return 0;
@@ -2108,8 +2214,9 @@ int interworking_home_sp_cred(struct wpa_supplicant *wpa_s,
                realm = os_strchr(nai, '@');
                if (realm)
                        realm++;
-               wpa_printf(MSG_DEBUG, "Interworking: Search for match "
-                          "with SIM/USIM domain %s", realm);
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Search for match with SIM/USIM domain %s",
+                       realm);
                if (realm &&
                    domain_name_list_contains(domain_names, realm, 1))
                        return 1;
@@ -2122,8 +2229,9 @@ int interworking_home_sp_cred(struct wpa_supplicant *wpa_s,
                return ret;
 
        for (i = 0; i < cred->num_domain; i++) {
-               wpa_printf(MSG_DEBUG, "Interworking: Search for match with "
-                          "home SP FQDN %s", cred->domain[i]);
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Search for match with home SP FQDN %s",
+                       cred->domain[i]);
                if (domain_name_list_contains(domain_names, cred->domain[i], 1))
                        return 1;
        }
@@ -2299,14 +2407,16 @@ static void interworking_select_network(struct wpa_supplicant *wpa_s)
                                                          &excluded);
                if (!cred)
                        continue;
+
                if (!wpa_bss_get_ie(bss, WLAN_EID_RSN)) {
                        /*
                         * We currently support only HS 2.0 networks and those
                         * are required to use WPA2-Enterprise.
                         */
-                       wpa_printf(MSG_DEBUG, "Interworking: Credential match "
-                                  "with " MACSTR " but network does not use "
-                                  "RSN", MAC2STR(bss->bssid));
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "Interworking: Credential match with " MACSTR
+                               " but network does not use RSN",
+                               MAC2STR(bss->bssid));
                        continue;
                }
                if (!excluded)
@@ -2397,8 +2507,8 @@ static void interworking_select_network(struct wpa_supplicant *wpa_s)
                 * have matching APs.
                 */
                if (interworking_find_network_match(wpa_s)) {
-                       wpa_printf(MSG_DEBUG, "Interworking: Possible BSS "
-                                  "match for enabled network configurations");
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "Interworking: Possible BSS match for enabled network configurations");
                        if (wpa_s->auto_select) {
                                interworking_reconnect(wpa_s);
                                return;
@@ -2406,8 +2516,8 @@ static void interworking_select_network(struct wpa_supplicant *wpa_s)
                }
 
                if (wpa_s->auto_network_select) {
-                       wpa_printf(MSG_DEBUG, "Interworking: Continue "
-                                  "scanning after ANQP fetch");
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "Interworking: Continue scanning after ANQP fetch");
                        wpa_supplicant_req_scan(wpa_s, wpa_s->scan_interval,
                                                0);
                        return;
@@ -2415,6 +2525,8 @@ static void interworking_select_network(struct wpa_supplicant *wpa_s)
 
                wpa_msg(wpa_s, MSG_INFO, INTERWORKING_NO_MATCH "No network "
                        "with matching credentials found");
+               if (wpa_s->wpa_state == WPA_SCANNING)
+                       wpa_supplicant_set_state(wpa_s, WPA_DISCONNECTED);
        }
 
        if (selected) {
@@ -2427,7 +2539,7 @@ static void interworking_select_network(struct wpa_supplicant *wpa_s)
                           MAC2STR(selected->bssid));
                wpa_msg(wpa_s, MSG_INFO, INTERWORKING_SELECTED MACSTR,
                        MAC2STR(selected->bssid));
-               interworking_connect(wpa_s, selected);
+               interworking_connect(wpa_s, selected, 0);
        }
 }
 
@@ -2458,9 +2570,10 @@ interworking_match_anqp_info(struct wpa_supplicant *wpa_s, struct wpa_bss *bss)
                    os_memcmp(bss->ssid, other->ssid, bss->ssid_len) != 0)
                        continue;
 
-               wpa_printf(MSG_DEBUG, "Interworking: Share ANQP data with "
-                          "already fetched BSSID " MACSTR " and " MACSTR,
-                          MAC2STR(other->bssid), MAC2STR(bss->bssid));
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Share ANQP data with already fetched BSSID "
+                       MACSTR " and " MACSTR,
+                       MAC2STR(other->bssid), MAC2STR(bss->bssid));
                other->anqp->users++;
                return other->anqp;
        }
@@ -2485,11 +2598,13 @@ static void interworking_next_anqp_fetch(struct wpa_supplicant *wpa_s)
                return;
        }
 
+#ifdef CONFIG_HS20
        if (wpa_s->fetch_osu_icon_in_progress) {
                wpa_printf(MSG_DEBUG, "Interworking: Next icon (in progress)");
                hs20_next_osu_icon(wpa_s);
                return;
        }
+#endif /* CONFIG_HS20 */
 
        dl_list_for_each(bss, &wpa_s->bss, struct wpa_bss, list) {
                if (!(bss->caps & IEEE80211_CAP_ESS))
@@ -2523,8 +2638,10 @@ static void interworking_next_anqp_fetch(struct wpa_supplicant *wpa_s)
        }
 
        if (found == 0) {
+#ifdef CONFIG_HS20
                if (wpa_s->fetch_osu_info) {
                        if (wpa_s->num_prov_found == 0 &&
+                           wpa_s->fetch_osu_waiting_scan &&
                            wpa_s->num_osu_scans < 3) {
                                wpa_printf(MSG_DEBUG, "HS 2.0: No OSU providers seen - try to scan again");
                                hs20_start_osu_scan(wpa_s);
@@ -2534,6 +2651,7 @@ static void interworking_next_anqp_fetch(struct wpa_supplicant *wpa_s)
                        hs20_osu_icon_fetch(wpa_s);
                        return;
                }
+#endif /* CONFIG_HS20 */
                wpa_msg(wpa_s, MSG_INFO, "ANQP fetch completed");
                wpa_s->fetch_anqp_in_progress = 0;
                if (wpa_s->network_select)
@@ -2550,7 +2668,12 @@ void interworking_start_fetch_anqp(struct wpa_supplicant *wpa_s)
                bss->flags &= ~WPA_BSS_ANQP_FETCH_TRIED;
 
        wpa_s->fetch_anqp_in_progress = 1;
-       interworking_next_anqp_fetch(wpa_s);
+
+       /*
+        * Start actual ANQP operation from eloop call to make sure the loop
+        * does not end up using excessive recursion.
+        */
+       eloop_register_timeout(0, 0, interworking_continue_anqp, wpa_s, NULL);
 }
 
 
@@ -2588,17 +2711,20 @@ int anqp_send_req(struct wpa_supplicant *wpa_s, const u8 *dst,
        struct wpa_bss *bss;
        int res;
 
-       freq = wpa_s->assoc_freq;
        bss = wpa_bss_get_bssid(wpa_s, dst);
-       if (bss) {
-               wpa_bss_anqp_unshare_alloc(bss);
-               freq = bss->freq;
-       }
-       if (freq <= 0)
+       if (!bss) {
+               wpa_printf(MSG_WARNING,
+                          "ANQP: Cannot send query to unknown BSS "
+                          MACSTR, MAC2STR(dst));
                return -1;
+       }
+
+       wpa_bss_anqp_unshare_alloc(bss);
+       freq = bss->freq;
 
-       wpa_printf(MSG_DEBUG, "ANQP: Query Request to " MACSTR " for %u id(s)",
-                  MAC2STR(dst), (unsigned int) num_ids);
+       wpa_msg(wpa_s, MSG_DEBUG,
+               "ANQP: Query Request to " MACSTR " for %u id(s)",
+               MAC2STR(dst), (unsigned int) num_ids);
 
 #ifdef CONFIG_HS20
        if (subtypes != 0) {
@@ -2616,21 +2742,58 @@ int anqp_send_req(struct wpa_supplicant *wpa_s, const u8 *dst,
 
        res = gas_query_req(wpa_s->gas, dst, freq, buf, anqp_resp_cb, wpa_s);
        if (res < 0) {
-               wpa_printf(MSG_DEBUG, "ANQP: Failed to send Query Request");
+               wpa_msg(wpa_s, MSG_DEBUG, "ANQP: Failed to send Query Request");
                wpabuf_free(buf);
                ret = -1;
-       } else
-               wpa_printf(MSG_DEBUG, "ANQP: Query started with dialog token "
-                          "%u", res);
+       } else {
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "ANQP: Query started with dialog token %u", res);
+       }
 
        return ret;
 }
 
 
+static void anqp_add_extra(struct wpa_supplicant *wpa_s,
+                          struct wpa_bss_anqp *anqp, u16 info_id,
+                          const u8 *data, size_t slen)
+{
+       struct wpa_bss_anqp_elem *tmp, *elem = NULL;
+
+       if (!anqp)
+               return;
+
+       dl_list_for_each(tmp, &anqp->anqp_elems, struct wpa_bss_anqp_elem,
+                        list) {
+               if (tmp->infoid == info_id) {
+                       elem = tmp;
+                       break;
+               }
+       }
+
+       if (!elem) {
+               elem = os_zalloc(sizeof(*elem));
+               if (!elem)
+                       return;
+               elem->infoid = info_id;
+               dl_list_add(&anqp->anqp_elems, &elem->list);
+       } else {
+               wpabuf_free(elem->payload);
+       }
+
+       elem->payload = wpabuf_alloc_copy(data, slen);
+       if (!elem->payload) {
+               dl_list_del(&elem->list);
+               os_free(elem);
+       }
+}
+
+
 static void interworking_parse_rx_anqp_resp(struct wpa_supplicant *wpa_s,
                                            struct wpa_bss *bss, const u8 *sa,
                                            u16 info_id,
-                                           const u8 *data, size_t slen)
+                                           const u8 *data, size_t slen,
+                                           u8 dialog_token)
 {
        const u8 *pos = data;
        struct wpa_bss_anqp *anqp = NULL;
@@ -2645,6 +2808,12 @@ static void interworking_parse_rx_anqp_resp(struct wpa_supplicant *wpa_s,
        case ANQP_CAPABILITY_LIST:
                wpa_msg(wpa_s, MSG_INFO, "RX-ANQP " MACSTR
                        " ANQP Capability list", MAC2STR(sa));
+               wpa_hexdump_ascii(MSG_DEBUG, "ANQP: Capability list",
+                                 pos, slen);
+               if (anqp) {
+                       wpabuf_free(anqp->capability_list);
+                       anqp->capability_list = wpabuf_alloc_copy(pos, slen);
+               }
                break;
        case ANQP_VENUE_NAME:
                wpa_msg(wpa_s, MSG_INFO, "RX-ANQP " MACSTR
@@ -2733,26 +2902,29 @@ static void interworking_parse_rx_anqp_resp(struct wpa_supplicant *wpa_s,
 
                        switch (type) {
                        case HS20_ANQP_OUI_TYPE:
-                               hs20_parse_rx_hs20_anqp_resp(wpa_s, sa, pos,
-                                                            slen);
+                               hs20_parse_rx_hs20_anqp_resp(wpa_s, bss, sa,
+                                                            pos, slen,
+                                                            dialog_token);
                                break;
                        default:
-                               wpa_printf(MSG_DEBUG, "HS20: Unsupported ANQP "
-                                          "vendor type %u", type);
+                               wpa_msg(wpa_s, MSG_DEBUG,
+                                       "HS20: Unsupported ANQP vendor type %u",
+                                       type);
                                break;
                        }
                        break;
 #endif /* CONFIG_HS20 */
                default:
-                       wpa_printf(MSG_DEBUG, "Interworking: Unsupported "
-                                  "vendor-specific ANQP OUI %06x",
-                                  WPA_GET_BE24(pos));
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "Interworking: Unsupported vendor-specific ANQP OUI %06x",
+                               WPA_GET_BE24(pos));
                        return;
                }
                break;
        default:
-               wpa_printf(MSG_DEBUG, "Interworking: Unsupported ANQP Info ID "
-                          "%u", info_id);
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "Interworking: Unsupported ANQP Info ID %u", info_id);
+               anqp_add_extra(wpa_s, anqp, info_id, data, slen);
                break;
        }
 }
@@ -2769,24 +2941,31 @@ void anqp_resp_cb(void *ctx, const u8 *dst, u8 dialog_token,
        u16 info_id;
        u16 slen;
        struct wpa_bss *bss = NULL, *tmp;
+       const char *anqp_result = "SUCCESS";
 
        wpa_printf(MSG_DEBUG, "Interworking: anqp_resp_cb dst=" MACSTR
                   " dialog_token=%u result=%d status_code=%u",
                   MAC2STR(dst), dialog_token, result, status_code);
        if (result != GAS_QUERY_SUCCESS) {
+#ifdef CONFIG_HS20
                if (wpa_s->fetch_osu_icon_in_progress)
                        hs20_icon_fetch_failed(wpa_s);
-               return;
+#endif /* CONFIG_HS20 */
+               anqp_result = "FAILURE";
+               goto out;
        }
 
        pos = wpabuf_head(adv_proto);
        if (wpabuf_len(adv_proto) < 4 || pos[0] != WLAN_EID_ADV_PROTO ||
            pos[1] < 2 || pos[3] != ACCESS_NETWORK_QUERY_PROTOCOL) {
-               wpa_printf(MSG_DEBUG, "ANQP: Unexpected Advertisement "
-                          "Protocol in response");
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "ANQP: Unexpected Advertisement Protocol in response");
+#ifdef CONFIG_HS20
                if (wpa_s->fetch_osu_icon_in_progress)
                        hs20_icon_fetch_failed(wpa_s);
-               return;
+#endif /* CONFIG_HS20 */
+               anqp_result = "INVALID_FRAME";
+               goto out;
        }
 
        /*
@@ -2808,33 +2987,45 @@ void anqp_resp_cb(void *ctx, const u8 *dst, u8 dialog_token,
        end = pos + wpabuf_len(resp);
 
        while (pos < end) {
-               if (pos + 4 > end) {
-                       wpa_printf(MSG_DEBUG, "ANQP: Invalid element");
-                       break;
+               unsigned int left = end - pos;
+
+               if (left < 4) {
+                       wpa_msg(wpa_s, MSG_DEBUG, "ANQP: Invalid element");
+                       anqp_result = "INVALID_FRAME";
+                       goto out_parse_done;
                }
                info_id = WPA_GET_LE16(pos);
                pos += 2;
                slen = WPA_GET_LE16(pos);
                pos += 2;
-               if (pos + slen > end) {
-                       wpa_printf(MSG_DEBUG, "ANQP: Invalid element length "
-                                  "for Info ID %u", info_id);
-                       break;
+               left -= 4;
+               if (left < slen) {
+                       wpa_msg(wpa_s, MSG_DEBUG,
+                               "ANQP: Invalid element length for Info ID %u",
+                               info_id);
+                       anqp_result = "INVALID_FRAME";
+                       goto out_parse_done;
                }
                interworking_parse_rx_anqp_resp(wpa_s, bss, dst, info_id, pos,
-                                               slen);
+                                               slen, dialog_token);
                pos += slen;
        }
 
+out_parse_done:
+#ifdef CONFIG_HS20
        hs20_notify_parse_done(wpa_s);
+#endif /* CONFIG_HS20 */
+out:
+       wpa_msg(wpa_s, MSG_INFO, ANQP_QUERY_DONE "addr=" MACSTR " result=%s",
+               MAC2STR(dst), anqp_result);
 }
 
 
 static void interworking_scan_res_handler(struct wpa_supplicant *wpa_s,
                                          struct wpa_scan_results *scan_res)
 {
-       wpa_printf(MSG_DEBUG, "Interworking: Scan results available - start "
-                  "ANQP fetch");
+       wpa_msg(wpa_s, MSG_DEBUG,
+               "Interworking: Scan results available - start ANQP fetch");
        interworking_start_fetch_anqp(wpa_s);
 }
 
@@ -2848,8 +3039,8 @@ int interworking_select(struct wpa_supplicant *wpa_s, int auto_select,
        wpa_s->auto_select = !!auto_select;
        wpa_s->fetch_all_anqp = 0;
        wpa_s->fetch_osu_info = 0;
-       wpa_printf(MSG_DEBUG, "Interworking: Start scan for network "
-                  "selection");
+       wpa_msg(wpa_s, MSG_DEBUG,
+               "Interworking: Start scan for network selection");
        wpa_s->scan_res_handler = interworking_scan_res_handler;
        wpa_s->normal_scans = 0;
        wpa_s->scan_req = MANUAL_SCAN_REQ;
@@ -2910,8 +3101,8 @@ int gas_send_request(struct wpa_supplicant *wpa_s, const u8 *dst,
        if (freq <= 0)
                return -1;
 
-       wpa_printf(MSG_DEBUG, "GAS request to " MACSTR " (freq %d MHz)",
-                  MAC2STR(dst), freq);
+       wpa_msg(wpa_s, MSG_DEBUG, "GAS request to " MACSTR " (freq %d MHz)",
+               MAC2STR(dst), freq);
        wpa_hexdump_buf(MSG_DEBUG, "Advertisement Protocol ID", adv_proto);
        wpa_hexdump_buf(MSG_DEBUG, "GAS Query", query);
 
@@ -2937,12 +3128,12 @@ int gas_send_request(struct wpa_supplicant *wpa_s, const u8 *dst,
 
        res = gas_query_req(wpa_s->gas, dst, freq, buf, gas_resp_cb, wpa_s);
        if (res < 0) {
-               wpa_printf(MSG_DEBUG, "GAS: Failed to send Query Request");
+               wpa_msg(wpa_s, MSG_DEBUG, "GAS: Failed to send Query Request");
                wpabuf_free(buf);
                ret = -1;
        } else
-               wpa_printf(MSG_DEBUG, "GAS: Query started with dialog token "
-                          "%u", res);
+               wpa_msg(wpa_s, MSG_DEBUG,
+                       "GAS: Query started with dialog token %u", res);
 
        return ret;
 }