LCOV - code coverage report
Current view: top level - src/p2p - p2p_sd.c (source / functions) Hit Total Coverage
Test: wpa_supplicant/hostapd combined for hwsim test run 1422976643 Lines: 401 521 77.0 %
Date: 2015-02-03 Functions: 20 20 100.0 %

          Line data    Source code
       1             : /*
       2             :  * Wi-Fi Direct - P2P service discovery
       3             :  * Copyright (c) 2009, Atheros Communications
       4             :  *
       5             :  * This software may be distributed under the terms of the BSD license.
       6             :  * See README for more details.
       7             :  */
       8             : 
       9             : #include "includes.h"
      10             : 
      11             : #include "common.h"
      12             : #include "common/ieee802_11_defs.h"
      13             : #include "common/gas.h"
      14             : #include "p2p_i.h"
      15             : #include "p2p.h"
      16             : 
      17             : 
      18             : #ifdef CONFIG_WIFI_DISPLAY
      19         943 : static int wfd_wsd_supported(struct wpabuf *wfd)
      20             : {
      21             :         const u8 *pos, *end;
      22             :         u8 subelem;
      23             :         u16 len;
      24             : 
      25         943 :         if (wfd == NULL)
      26         926 :                 return 0;
      27             : 
      28          17 :         pos = wpabuf_head(wfd);
      29          17 :         end = pos + wpabuf_len(wfd);
      30             : 
      31          34 :         while (pos + 3 <= end) {
      32          17 :                 subelem = *pos++;
      33          17 :                 len = WPA_GET_BE16(pos);
      34          17 :                 pos += 2;
      35          17 :                 if (pos + len > end)
      36           1 :                         break;
      37             : 
      38          16 :                 if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) {
      39          16 :                         u16 info = WPA_GET_BE16(pos);
      40          16 :                         return !!(info & 0x0040);
      41             :                 }
      42             : 
      43           0 :                 pos += len;
      44             :         }
      45             : 
      46           1 :         return 0;
      47             : }
      48             : #endif /* CONFIG_WIFI_DISPLAY */
      49             : 
      50         943 : struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
      51             :                                          struct p2p_device *dev)
      52             : {
      53             :         struct p2p_sd_query *q;
      54         943 :         int wsd = 0;
      55         943 :         int count = 0;
      56             : 
      57         943 :         if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
      58           0 :                 return NULL; /* peer does not support SD */
      59             : #ifdef CONFIG_WIFI_DISPLAY
      60         943 :         if (wfd_wsd_supported(dev->info.wfd_subelems))
      61          11 :                 wsd = 1;
      62             : #endif /* CONFIG_WIFI_DISPLAY */
      63             : 
      64         953 :         for (q = p2p->sd_queries; q; q = q->next) {
      65             :                 /* Use WSD only if the peer indicates support or it */
      66         117 :                 if (q->wsd && !wsd)
      67           0 :                         continue;
      68             :                 /* if the query is a broadcast query */
      69         117 :                 if (q->for_all_peers) {
      70             :                         /*
      71             :                          * check if there are any broadcast queries pending for
      72             :                          * this device
      73             :                          */
      74          93 :                         if (dev->sd_pending_bcast_queries <= 0)
      75          47 :                                 return NULL;
      76             :                         /* query number that needs to be send to the device */
      77          46 :                         if (count == dev->sd_pending_bcast_queries - 1)
      78          42 :                                 goto found;
      79           4 :                         count++;
      80             :                 }
      81          52 :                 if (!q->for_all_peers &&
      82          24 :                     os_memcmp(q->peer, dev->info.p2p_device_addr, ETH_ALEN) ==
      83             :                     0)
      84          18 :                         goto found;
      85             :         }
      86             : 
      87         836 :         return NULL;
      88             : 
      89             : found:
      90          60 :         if (dev->sd_reqs > 100) {
      91           0 :                 p2p_dbg(p2p, "Too many SD request attempts to " MACSTR
      92             :                         " - skip remaining queries",
      93           0 :                         MAC2STR(dev->info.p2p_device_addr));
      94           0 :                 return NULL;
      95             :         }
      96          60 :         return q;
      97             : }
      98             : 
      99             : 
     100           5 : static void p2p_decrease_sd_bc_queries(struct p2p_data *p2p, int query_number)
     101             : {
     102             :         struct p2p_device *dev;
     103             : 
     104           5 :         p2p->num_p2p_sd_queries--;
     105           9 :         dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) {
     106           4 :                 if (query_number <= dev->sd_pending_bcast_queries - 1) {
     107             :                         /*
     108             :                          * Query not yet sent to the device and it is to be
     109             :                          * removed, so update the pending count.
     110             :                         */
     111           2 :                         dev->sd_pending_bcast_queries--;
     112             :                 }
     113             :         }
     114           5 : }
     115             : 
     116             : 
     117          29 : static int p2p_unlink_sd_query(struct p2p_data *p2p,
     118             :                                struct p2p_sd_query *query)
     119             : {
     120             :         struct p2p_sd_query *q, *prev;
     121          29 :         int query_number = 0;
     122             : 
     123          29 :         q = p2p->sd_queries;
     124          29 :         prev = NULL;
     125          61 :         while (q) {
     126          30 :                 if (q == query) {
     127             :                         /* If the query is a broadcast query, decrease one from
     128             :                          * all the devices */
     129          27 :                         if (query->for_all_peers)
     130           5 :                                 p2p_decrease_sd_bc_queries(p2p, query_number);
     131          27 :                         if (prev)
     132           3 :                                 prev->next = q->next;
     133             :                         else
     134          24 :                                 p2p->sd_queries = q->next;
     135          27 :                         if (p2p->sd_query == query)
     136          19 :                                 p2p->sd_query = NULL;
     137          27 :                         return 1;
     138             :                 }
     139           3 :                 if (q->for_all_peers)
     140           1 :                         query_number++;
     141           3 :                 prev = q;
     142           3 :                 q = q->next;
     143             :         }
     144           2 :         return 0;
     145             : }
     146             : 
     147             : 
     148          63 : static void p2p_free_sd_query(struct p2p_sd_query *q)
     149             : {
     150          63 :         if (q == NULL)
     151          63 :                 return;
     152          63 :         wpabuf_free(q->tlvs);
     153          63 :         os_free(q);
     154             : }
     155             : 
     156             : 
     157        3307 : void p2p_free_sd_queries(struct p2p_data *p2p)
     158             : {
     159             :         struct p2p_sd_query *q, *prev;
     160        3307 :         q = p2p->sd_queries;
     161        3307 :         p2p->sd_queries = NULL;
     162        6650 :         while (q) {
     163          36 :                 prev = q;
     164          36 :                 q = q->next;
     165          36 :                 p2p_free_sd_query(prev);
     166             :         }
     167        3307 :         p2p->num_p2p_sd_queries = 0;
     168        3307 : }
     169             : 
     170             : 
     171          60 : static struct wpabuf * p2p_build_sd_query(u16 update_indic,
     172             :                                           struct wpabuf *tlvs)
     173             : {
     174             :         struct wpabuf *buf;
     175             :         u8 *len_pos;
     176             : 
     177          60 :         buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs));
     178          60 :         if (buf == NULL)
     179           0 :                 return NULL;
     180             : 
     181             :         /* ANQP Query Request Frame */
     182          60 :         len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
     183          60 :         wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE);
     184          60 :         wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
     185          60 :         wpabuf_put_buf(buf, tlvs);
     186          60 :         gas_anqp_set_element_len(buf, len_pos);
     187             : 
     188          60 :         gas_anqp_set_len(buf);
     189             : 
     190          60 :         return buf;
     191             : }
     192             : 
     193             : 
     194          18 : static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst,
     195             :                                       u8 dialog_token, int freq)
     196             : {
     197             :         struct wpabuf *req;
     198             : 
     199          18 :         req = gas_build_comeback_req(dialog_token);
     200          18 :         if (req == NULL)
     201          18 :                 return;
     202             : 
     203          18 :         p2p->pending_action_state = P2P_NO_PENDING_ACTION;
     204          36 :         if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst,
     205          18 :                             wpabuf_head(req), wpabuf_len(req), 200) < 0)
     206           0 :                 p2p_dbg(p2p, "Failed to send Action frame");
     207             : 
     208          18 :         wpabuf_free(req);
     209             : }
     210             : 
     211             : 
     212          60 : static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
     213             :                                              u16 comeback_delay,
     214             :                                              u16 update_indic,
     215             :                                              const struct wpabuf *tlvs)
     216             : {
     217             :         struct wpabuf *buf;
     218             :         u8 *len_pos;
     219             : 
     220         117 :         buf = gas_anqp_build_initial_resp(dialog_token, status_code,
     221             :                                           comeback_delay,
     222          57 :                                           100 + (tlvs ? wpabuf_len(tlvs) : 0));
     223          60 :         if (buf == NULL)
     224           0 :                 return NULL;
     225             : 
     226          60 :         if (tlvs) {
     227             :                 /* ANQP Query Response Frame */
     228          57 :                 len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
     229          57 :                 wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE);
     230             :                  /* Service Update Indicator */
     231          57 :                 wpabuf_put_le16(buf, update_indic);
     232          57 :                 wpabuf_put_buf(buf, tlvs);
     233          57 :                 gas_anqp_set_element_len(buf, len_pos);
     234             :         }
     235             : 
     236          60 :         gas_anqp_set_len(buf);
     237             : 
     238          60 :         return buf;
     239             : }
     240             : 
     241             : 
     242          18 : static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
     243             :                                                    u16 status_code,
     244             :                                                    u16 update_indic,
     245             :                                                    const u8 *data, size_t len,
     246             :                                                    u8 frag_id, u8 more,
     247             :                                                    u16 total_len)
     248             : {
     249             :         struct wpabuf *buf;
     250             : 
     251          18 :         buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id,
     252             :                                            more, 0, 100 + len);
     253          18 :         if (buf == NULL)
     254           0 :                 return NULL;
     255             : 
     256          18 :         if (frag_id == 0) {
     257             :                 /* ANQP Query Response Frame */
     258           3 :                 wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */
     259           3 :                 wpabuf_put_le16(buf, 3 + 1 + 2 + total_len);
     260           3 :                 wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE);
     261             :                 /* Service Update Indicator */
     262           3 :                 wpabuf_put_le16(buf, update_indic);
     263             :         }
     264             : 
     265          18 :         wpabuf_put_data(buf, data, len);
     266          18 :         gas_anqp_set_len(buf);
     267             : 
     268          18 :         return buf;
     269             : }
     270             : 
     271             : 
     272         944 : int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
     273             : {
     274             :         struct wpabuf *req;
     275         944 :         int ret = 0;
     276             :         struct p2p_sd_query *query;
     277             :         int freq;
     278             :         unsigned int wait_time;
     279             : 
     280         944 :         freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
     281         944 :         if (freq <= 0) {
     282           6 :                 p2p_dbg(p2p, "No Listen/Operating frequency known for the peer "
     283             :                         MACSTR " to send SD Request",
     284           6 :                         MAC2STR(dev->info.p2p_device_addr));
     285           1 :                 return -1;
     286             :         }
     287             : 
     288         943 :         query = p2p_pending_sd_req(p2p, dev);
     289         943 :         if (query == NULL)
     290         883 :                 return -1;
     291             : 
     292         360 :         p2p_dbg(p2p, "Start Service Discovery with " MACSTR,
     293         360 :                 MAC2STR(dev->info.p2p_device_addr));
     294             : 
     295          60 :         req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
     296          60 :         if (req == NULL)
     297           0 :                 return -1;
     298             : 
     299          60 :         dev->sd_reqs++;
     300          60 :         p2p->sd_peer = dev;
     301          60 :         p2p->sd_query = query;
     302          60 :         p2p->pending_action_state = P2P_PENDING_SD;
     303             : 
     304          60 :         wait_time = 5000;
     305          60 :         if (p2p->cfg->max_listen && wait_time > p2p->cfg->max_listen)
     306           0 :                 wait_time = p2p->cfg->max_listen;
     307         120 :         if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr,
     308          60 :                             p2p->cfg->dev_addr, dev->info.p2p_device_addr,
     309          60 :                             wpabuf_head(req), wpabuf_len(req), wait_time) < 0) {
     310           0 :                 p2p_dbg(p2p, "Failed to send Action frame");
     311           0 :                 ret = -1;
     312             :         }
     313             : 
     314          60 :         wpabuf_free(req);
     315             : 
     316          60 :         return ret;
     317             : }
     318             : 
     319             : 
     320          60 : void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
     321             :                             const u8 *data, size_t len, int rx_freq)
     322             : {
     323          60 :         const u8 *pos = data;
     324          60 :         const u8 *end = data + len;
     325             :         const u8 *next;
     326             :         u8 dialog_token;
     327             :         u16 slen;
     328             :         int freq;
     329             :         u16 update_indic;
     330             : 
     331             : 
     332          60 :         if (p2p->cfg->sd_request == NULL)
     333           0 :                 return;
     334             : 
     335          60 :         if (rx_freq > 0)
     336          60 :                 freq = rx_freq;
     337             :         else
     338           0 :                 freq = p2p_channel_to_freq(p2p->cfg->reg_class,
     339           0 :                                            p2p->cfg->channel);
     340          60 :         if (freq < 0)
     341           0 :                 return;
     342             : 
     343          60 :         if (len < 1 + 2)
     344           0 :                 return;
     345             : 
     346          60 :         dialog_token = *pos++;
     347         420 :         p2p_dbg(p2p, "GAS Initial Request from " MACSTR
     348             :                 " (dialog token %u, freq %d)",
     349         360 :                 MAC2STR(sa), dialog_token, rx_freq);
     350             : 
     351          60 :         if (*pos != WLAN_EID_ADV_PROTO) {
     352           0 :                 p2p_dbg(p2p, "Unexpected IE in GAS Initial Request: %u", *pos);
     353           0 :                 return;
     354             :         }
     355          60 :         pos++;
     356             : 
     357          60 :         slen = *pos++;
     358          60 :         next = pos + slen;
     359          60 :         if (next > end || slen < 2) {
     360           0 :                 p2p_dbg(p2p, "Invalid IE in GAS Initial Request");
     361           0 :                 return;
     362             :         }
     363          60 :         pos++; /* skip QueryRespLenLimit and PAME-BI */
     364             : 
     365          60 :         if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
     366           0 :                 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
     367           0 :                         *pos);
     368           0 :                 return;
     369             :         }
     370             : 
     371          60 :         pos = next;
     372             :         /* Query Request */
     373          60 :         if (pos + 2 > end)
     374           0 :                 return;
     375          60 :         slen = WPA_GET_LE16(pos);
     376          60 :         pos += 2;
     377          60 :         if (pos + slen > end)
     378           0 :                 return;
     379          60 :         end = pos + slen;
     380             : 
     381             :         /* ANQP Query Request */
     382          60 :         if (pos + 4 > end)
     383           0 :                 return;
     384          60 :         if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
     385           0 :                 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
     386           0 :                 return;
     387             :         }
     388          60 :         pos += 2;
     389             : 
     390          60 :         slen = WPA_GET_LE16(pos);
     391          60 :         pos += 2;
     392          60 :         if (pos + slen > end || slen < 3 + 1) {
     393           0 :                 p2p_dbg(p2p, "Invalid ANQP Query Request length");
     394           0 :                 return;
     395             :         }
     396             : 
     397          60 :         if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
     398           0 :                 p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
     399             :                         WPA_GET_BE32(pos));
     400           0 :                 return;
     401             :         }
     402          60 :         pos += 4;
     403             : 
     404          60 :         if (pos + 2 > end)
     405           0 :                 return;
     406          60 :         update_indic = WPA_GET_LE16(pos);
     407          60 :         p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
     408          60 :         pos += 2;
     409             : 
     410         120 :         p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
     411          60 :                              update_indic, pos, end - pos);
     412             :         /* the response will be indicated with a call to p2p_sd_response() */
     413             : }
     414             : 
     415             : 
     416          60 : void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
     417             :                      u8 dialog_token, const struct wpabuf *resp_tlvs)
     418             : {
     419             :         struct wpabuf *resp;
     420             : 
     421             :         /* TODO: fix the length limit to match with the maximum frame length */
     422          60 :         if (wpabuf_len(resp_tlvs) > 1400) {
     423           3 :                 p2p_dbg(p2p, "SD response long enough to require fragmentation");
     424           3 :                 if (p2p->sd_resp) {
     425             :                         /*
     426             :                          * TODO: Could consider storing the fragmented response
     427             :                          * separately for each peer to avoid having to drop old
     428             :                          * one if there is more than one pending SD query.
     429             :                          * Though, that would eat more memory, so there are
     430             :                          * also benefits to just using a single buffer.
     431             :                          */
     432           0 :                         p2p_dbg(p2p, "Drop previous SD response");
     433           0 :                         wpabuf_free(p2p->sd_resp);
     434             :                 }
     435           3 :                 p2p->sd_resp = wpabuf_dup(resp_tlvs);
     436           3 :                 if (p2p->sd_resp == NULL) {
     437           0 :                         p2p_err(p2p, "Failed to allocate SD response fragmentation area");
     438           0 :                         return;
     439             :                 }
     440           3 :                 os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
     441           3 :                 p2p->sd_resp_dialog_token = dialog_token;
     442           3 :                 p2p->sd_resp_pos = 0;
     443           3 :                 p2p->sd_frag_id = 0;
     444           3 :                 resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
     445           3 :                                              1, p2p->srv_update_indic, NULL);
     446             :         } else {
     447          57 :                 p2p_dbg(p2p, "SD response fits in initial response");
     448          57 :                 resp = p2p_build_sd_response(dialog_token,
     449             :                                              WLAN_STATUS_SUCCESS, 0,
     450          57 :                                              p2p->srv_update_indic, resp_tlvs);
     451             :         }
     452          60 :         if (resp == NULL)
     453           0 :                 return;
     454             : 
     455          60 :         p2p->pending_action_state = P2P_NO_PENDING_ACTION;
     456         120 :         if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr,
     457          60 :                             p2p->cfg->dev_addr,
     458          60 :                             wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
     459           0 :                 p2p_dbg(p2p, "Failed to send Action frame");
     460             : 
     461          60 :         wpabuf_free(resp);
     462             : }
     463             : 
     464             : 
     465          60 : void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
     466             :                              const u8 *data, size_t len, int rx_freq)
     467             : {
     468          60 :         const u8 *pos = data;
     469          60 :         const u8 *end = data + len;
     470             :         const u8 *next;
     471             :         u8 dialog_token;
     472             :         u16 status_code;
     473             :         u16 comeback_delay;
     474             :         u16 slen;
     475             :         u16 update_indic;
     476             : 
     477         120 :         if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
     478          60 :             os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
     479           0 :                 p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from "
     480           0 :                         MACSTR, MAC2STR(sa));
     481           0 :                 return;
     482             :         }
     483          60 :         p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
     484          60 :         p2p_clear_timeout(p2p);
     485             : 
     486         420 :         p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)",
     487         360 :                 MAC2STR(sa), (int) len);
     488             : 
     489          60 :         if (len < 5 + 2) {
     490           0 :                 p2p_dbg(p2p, "Too short GAS Initial Response frame");
     491           0 :                 return;
     492             :         }
     493             : 
     494          60 :         dialog_token = *pos++;
     495             :         /* TODO: check dialog_token match */
     496          60 :         status_code = WPA_GET_LE16(pos);
     497          60 :         pos += 2;
     498          60 :         comeback_delay = WPA_GET_LE16(pos);
     499          60 :         pos += 2;
     500          60 :         p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u",
     501             :                 dialog_token, status_code, comeback_delay);
     502          60 :         if (status_code) {
     503           0 :                 p2p_dbg(p2p, "Service Discovery failed: status code %u",
     504             :                         status_code);
     505           0 :                 return;
     506             :         }
     507             : 
     508          60 :         if (*pos != WLAN_EID_ADV_PROTO) {
     509           0 :                 p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos);
     510           0 :                 return;
     511             :         }
     512          60 :         pos++;
     513             : 
     514          60 :         slen = *pos++;
     515          60 :         next = pos + slen;
     516          60 :         if (next > end || slen < 2) {
     517           0 :                 p2p_dbg(p2p, "Invalid IE in GAS Initial Response");
     518           0 :                 return;
     519             :         }
     520          60 :         pos++; /* skip QueryRespLenLimit and PAME-BI */
     521             : 
     522          60 :         if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
     523           0 :                 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
     524           0 :                         *pos);
     525           0 :                 return;
     526             :         }
     527             : 
     528          60 :         pos = next;
     529             :         /* Query Response */
     530          60 :         if (pos + 2 > end) {
     531           0 :                 p2p_dbg(p2p, "Too short Query Response");
     532           0 :                 return;
     533             :         }
     534          60 :         slen = WPA_GET_LE16(pos);
     535          60 :         pos += 2;
     536          60 :         p2p_dbg(p2p, "Query Response Length: %d", slen);
     537          60 :         if (pos + slen > end) {
     538           0 :                 p2p_dbg(p2p, "Not enough Query Response data");
     539           0 :                 return;
     540             :         }
     541          60 :         end = pos + slen;
     542             : 
     543          60 :         if (comeback_delay) {
     544           3 :                 p2p_dbg(p2p, "Fragmented response - request fragments");
     545           3 :                 if (p2p->sd_rx_resp) {
     546           0 :                         p2p_dbg(p2p, "Drop old SD reassembly buffer");
     547           0 :                         wpabuf_free(p2p->sd_rx_resp);
     548           0 :                         p2p->sd_rx_resp = NULL;
     549             :                 }
     550           3 :                 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
     551           3 :                 return;
     552             :         }
     553             : 
     554             :         /* ANQP Query Response */
     555          57 :         if (pos + 4 > end)
     556           0 :                 return;
     557          57 :         if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
     558           0 :                 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
     559           0 :                 return;
     560             :         }
     561          57 :         pos += 2;
     562             : 
     563          57 :         slen = WPA_GET_LE16(pos);
     564          57 :         pos += 2;
     565          57 :         if (pos + slen > end || slen < 3 + 1) {
     566           0 :                 p2p_dbg(p2p, "Invalid ANQP Query Response length");
     567           0 :                 return;
     568             :         }
     569             : 
     570          57 :         if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
     571           0 :                 p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
     572             :                         WPA_GET_BE32(pos));
     573           0 :                 return;
     574             :         }
     575          57 :         pos += 4;
     576             : 
     577          57 :         if (pos + 2 > end)
     578           0 :                 return;
     579          57 :         update_indic = WPA_GET_LE16(pos);
     580          57 :         p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
     581          57 :         pos += 2;
     582             : 
     583          57 :         p2p->sd_peer = NULL;
     584             : 
     585          57 :         if (p2p->sd_query) {
     586          57 :                 if (!p2p->sd_query->for_all_peers) {
     587             :                         struct p2p_sd_query *q;
     588          17 :                         p2p_dbg(p2p, "Remove completed SD query %p",
     589             :                                 p2p->sd_query);
     590          17 :                         q = p2p->sd_query;
     591          17 :                         p2p_unlink_sd_query(p2p, p2p->sd_query);
     592          17 :                         p2p_free_sd_query(q);
     593             :                 }
     594          57 :                 p2p->sd_query = NULL;
     595             :         }
     596             : 
     597          57 :         if (p2p->cfg->sd_response)
     598         114 :                 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
     599          57 :                                       pos, end - pos);
     600          57 :         p2p_continue_find(p2p);
     601             : }
     602             : 
     603             : 
     604          18 : void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
     605             :                              const u8 *data, size_t len, int rx_freq)
     606             : {
     607             :         struct wpabuf *resp;
     608             :         u8 dialog_token;
     609             :         size_t frag_len;
     610          18 :         int more = 0;
     611             : 
     612          18 :         wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
     613          18 :         if (len < 1)
     614           0 :                 return;
     615          18 :         dialog_token = *data;
     616          18 :         p2p_dbg(p2p, "Dialog Token: %u", dialog_token);
     617          18 :         if (dialog_token != p2p->sd_resp_dialog_token) {
     618           0 :                 p2p_dbg(p2p, "No pending SD response fragment for dialog token %u",
     619             :                         dialog_token);
     620           0 :                 return;
     621             :         }
     622             : 
     623          18 :         if (p2p->sd_resp == NULL) {
     624           0 :                 p2p_dbg(p2p, "No pending SD response fragment available");
     625           0 :                 return;
     626             :         }
     627          18 :         if (os_memcmp(sa, p2p->sd_resp_addr, ETH_ALEN) != 0) {
     628           0 :                 p2p_dbg(p2p, "No pending SD response fragment for " MACSTR,
     629           0 :                         MAC2STR(sa));
     630           0 :                 return;
     631             :         }
     632             : 
     633          18 :         frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
     634          18 :         if (frag_len > 1400) {
     635          15 :                 frag_len = 1400;
     636          15 :                 more = 1;
     637             :         }
     638          72 :         resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
     639          18 :                                            p2p->srv_update_indic,
     640          18 :                                            wpabuf_head_u8(p2p->sd_resp) +
     641             :                                            p2p->sd_resp_pos, frag_len,
     642          18 :                                            p2p->sd_frag_id, more,
     643          18 :                                            wpabuf_len(p2p->sd_resp));
     644          18 :         if (resp == NULL)
     645           0 :                 return;
     646          36 :         p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)",
     647          18 :                 p2p->sd_frag_id, more, (int) frag_len);
     648          18 :         p2p->sd_frag_id++;
     649          18 :         p2p->sd_resp_pos += frag_len;
     650             : 
     651          18 :         if (more) {
     652          30 :                 p2p_dbg(p2p, "%d more bytes remain to be sent",
     653          30 :                         (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
     654             :         } else {
     655           3 :                 p2p_dbg(p2p, "All fragments of SD response sent");
     656           3 :                 wpabuf_free(p2p->sd_resp);
     657           3 :                 p2p->sd_resp = NULL;
     658             :         }
     659             : 
     660          18 :         p2p->pending_action_state = P2P_NO_PENDING_ACTION;
     661          36 :         if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
     662          18 :                             p2p->cfg->dev_addr,
     663          18 :                             wpabuf_head(resp), wpabuf_len(resp), 200) < 0)
     664           0 :                 p2p_dbg(p2p, "Failed to send Action frame");
     665             : 
     666          18 :         wpabuf_free(resp);
     667             : }
     668             : 
     669             : 
     670          18 : void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
     671             :                               const u8 *data, size_t len, int rx_freq)
     672             : {
     673          18 :         const u8 *pos = data;
     674          18 :         const u8 *end = data + len;
     675             :         const u8 *next;
     676             :         u8 dialog_token;
     677             :         u16 status_code;
     678             :         u8 frag_id;
     679             :         u8 more_frags;
     680             :         u16 comeback_delay;
     681             :         u16 slen;
     682             : 
     683          18 :         wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);
     684             : 
     685          36 :         if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
     686          18 :             os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
     687           0 :                 p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from "
     688           0 :                         MACSTR, MAC2STR(sa));
     689           0 :                 return;
     690             :         }
     691          18 :         p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
     692          18 :         p2p_clear_timeout(p2p);
     693             : 
     694         126 :         p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)",
     695         108 :                 MAC2STR(sa), (int) len);
     696             : 
     697          18 :         if (len < 6 + 2) {
     698           0 :                 p2p_dbg(p2p, "Too short GAS Comeback Response frame");
     699           0 :                 return;
     700             :         }
     701             : 
     702          18 :         dialog_token = *pos++;
     703             :         /* TODO: check dialog_token match */
     704          18 :         status_code = WPA_GET_LE16(pos);
     705          18 :         pos += 2;
     706          18 :         frag_id = *pos & 0x7f;
     707          18 :         more_frags = (*pos & 0x80) >> 7;
     708          18 :         pos++;
     709          18 :         comeback_delay = WPA_GET_LE16(pos);
     710          18 :         pos += 2;
     711          18 :         p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
     712             :                 "comeback_delay=%u",
     713             :                 dialog_token, status_code, frag_id, more_frags,
     714             :                 comeback_delay);
     715             :         /* TODO: check frag_id match */
     716          18 :         if (status_code) {
     717           0 :                 p2p_dbg(p2p, "Service Discovery failed: status code %u",
     718             :                         status_code);
     719           0 :                 return;
     720             :         }
     721             : 
     722          18 :         if (*pos != WLAN_EID_ADV_PROTO) {
     723           0 :                 p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u",
     724           0 :                         *pos);
     725           0 :                 return;
     726             :         }
     727          18 :         pos++;
     728             : 
     729          18 :         slen = *pos++;
     730          18 :         next = pos + slen;
     731          18 :         if (next > end || slen < 2) {
     732           0 :                 p2p_dbg(p2p, "Invalid IE in GAS Comeback Response");
     733           0 :                 return;
     734             :         }
     735          18 :         pos++; /* skip QueryRespLenLimit and PAME-BI */
     736             : 
     737          18 :         if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
     738           0 :                 p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
     739           0 :                         *pos);
     740           0 :                 return;
     741             :         }
     742             : 
     743          18 :         pos = next;
     744             :         /* Query Response */
     745          18 :         if (pos + 2 > end) {
     746           0 :                 p2p_dbg(p2p, "Too short Query Response");
     747           0 :                 return;
     748             :         }
     749          18 :         slen = WPA_GET_LE16(pos);
     750          18 :         pos += 2;
     751          18 :         p2p_dbg(p2p, "Query Response Length: %d", slen);
     752          18 :         if (pos + slen > end) {
     753           0 :                 p2p_dbg(p2p, "Not enough Query Response data");
     754           0 :                 return;
     755             :         }
     756          18 :         if (slen == 0) {
     757           0 :                 p2p_dbg(p2p, "No Query Response data");
     758           0 :                 return;
     759             :         }
     760          18 :         end = pos + slen;
     761             : 
     762          18 :         if (p2p->sd_rx_resp) {
     763             :                  /*
     764             :                   * ANQP header is only included in the first fragment; rest of
     765             :                   * the fragments start with continue TLVs.
     766             :                   */
     767          15 :                 goto skip_nqp_header;
     768             :         }
     769             : 
     770             :         /* ANQP Query Response */
     771           3 :         if (pos + 4 > end)
     772           0 :                 return;
     773           3 :         if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
     774           0 :                 p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
     775           0 :                 return;
     776             :         }
     777           3 :         pos += 2;
     778             : 
     779           3 :         slen = WPA_GET_LE16(pos);
     780           3 :         pos += 2;
     781           3 :         p2p_dbg(p2p, "ANQP Query Response length: %u", slen);
     782           3 :         if (slen < 3 + 1) {
     783           0 :                 p2p_dbg(p2p, "Invalid ANQP Query Response length");
     784           0 :                 return;
     785             :         }
     786           3 :         if (pos + 4 > end)
     787           0 :                 return;
     788             : 
     789           3 :         if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
     790           0 :                 p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
     791             :                         WPA_GET_BE32(pos));
     792           0 :                 return;
     793             :         }
     794           3 :         pos += 4;
     795             : 
     796           3 :         if (pos + 2 > end)
     797           0 :                 return;
     798           3 :         p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
     799           3 :         p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic);
     800           3 :         pos += 2;
     801             : 
     802             : skip_nqp_header:
     803          18 :         if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
     804           0 :                 return;
     805          18 :         wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
     806          18 :         p2p_dbg(p2p, "Current SD reassembly buffer length: %u",
     807          18 :                 (unsigned int) wpabuf_len(p2p->sd_rx_resp));
     808             : 
     809          18 :         if (more_frags) {
     810          15 :                 p2p_dbg(p2p, "More fragments remains");
     811             :                 /* TODO: what would be a good size limit? */
     812          15 :                 if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
     813           0 :                         wpabuf_free(p2p->sd_rx_resp);
     814           0 :                         p2p->sd_rx_resp = NULL;
     815           0 :                         p2p_dbg(p2p, "Too long SD response - drop it");
     816           0 :                         return;
     817             :                 }
     818          15 :                 p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
     819          15 :                 return;
     820             :         }
     821             : 
     822           3 :         p2p->sd_peer = NULL;
     823             : 
     824           3 :         if (p2p->sd_query) {
     825           2 :                 if (!p2p->sd_query->for_all_peers) {
     826             :                         struct p2p_sd_query *q;
     827           1 :                         p2p_dbg(p2p, "Remove completed SD query %p",
     828             :                                 p2p->sd_query);
     829           1 :                         q = p2p->sd_query;
     830           1 :                         p2p_unlink_sd_query(p2p, p2p->sd_query);
     831           1 :                         p2p_free_sd_query(q);
     832             :                 }
     833           2 :                 p2p->sd_query = NULL;
     834             :         }
     835             : 
     836           3 :         if (p2p->cfg->sd_response)
     837           9 :                 p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
     838           3 :                                       p2p->sd_rx_update_indic,
     839           3 :                                       wpabuf_head(p2p->sd_rx_resp),
     840           3 :                                       wpabuf_len(p2p->sd_rx_resp));
     841           3 :         wpabuf_free(p2p->sd_rx_resp);
     842           3 :         p2p->sd_rx_resp = NULL;
     843             : 
     844           3 :         p2p_continue_find(p2p);
     845             : }
     846             : 
     847             : 
     848          63 : void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
     849             :                       const struct wpabuf *tlvs)
     850             : {
     851             :         struct p2p_sd_query *q;
     852             : 
     853          63 :         q = os_zalloc(sizeof(*q));
     854          63 :         if (q == NULL)
     855           0 :                 return NULL;
     856             : 
     857          63 :         if (dst)
     858          22 :                 os_memcpy(q->peer, dst, ETH_ALEN);
     859             :         else
     860          41 :                 q->for_all_peers = 1;
     861             : 
     862          63 :         q->tlvs = wpabuf_dup(tlvs);
     863          63 :         if (q->tlvs == NULL) {
     864           0 :                 p2p_free_sd_query(q);
     865           0 :                 return NULL;
     866             :         }
     867             : 
     868          63 :         q->next = p2p->sd_queries;
     869          63 :         p2p->sd_queries = q;
     870          63 :         p2p_dbg(p2p, "Added SD Query %p", q);
     871             : 
     872          63 :         if (dst == NULL) {
     873             :                 struct p2p_device *dev;
     874             : 
     875          41 :                 p2p->num_p2p_sd_queries++;
     876             : 
     877             :                 /* Update all the devices for the newly added broadcast query */
     878          42 :                 dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) {
     879           1 :                         if (dev->sd_pending_bcast_queries <= 0)
     880           1 :                                 dev->sd_pending_bcast_queries = 1;
     881             :                         else
     882           0 :                                 dev->sd_pending_bcast_queries++;
     883             :                 }
     884             :         }
     885             : 
     886          63 :         return q;
     887             : }
     888             : 
     889             : 
     890             : #ifdef CONFIG_WIFI_DISPLAY
     891           1 : void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
     892             :                           const struct wpabuf *tlvs)
     893             : {
     894             :         struct p2p_sd_query *q;
     895           1 :         q = p2p_sd_request(p2p, dst, tlvs);
     896           1 :         if (q)
     897           1 :                 q->wsd = 1;
     898           1 :         return q;
     899             : }
     900             : #endif /* CONFIG_WIFI_DISPLAY */
     901             : 
     902             : 
     903        3899 : void p2p_sd_service_update(struct p2p_data *p2p)
     904             : {
     905        3899 :         p2p->srv_update_indic++;
     906        3899 : }
     907             : 
     908             : 
     909          11 : int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
     910             : {
     911          11 :         if (p2p_unlink_sd_query(p2p, req)) {
     912           9 :                 p2p_dbg(p2p, "Cancel pending SD query %p", req);
     913           9 :                 p2p_free_sd_query(req);
     914           9 :                 return 0;
     915             :         }
     916           2 :         return -1;
     917             : }

Generated by: LCOV version 1.10