/src/hostap/src/p2p/p2p_sd.c

Source (jump to first uncovered line)
/*
 * Wi-Fi Direct - P2P service discovery
 * Copyright (c) 2009, Atheros Communications
 *
 * This software may be distributed under the terms of the BSD license.
 * See README for more details.
 */

#include "includes.h"

#include "common.h"
#include "common/ieee802_11_defs.h"
#include "common/gas.h"
#include "p2p_i.h"
#include "p2p.h"


#ifdef CONFIG_WIFI_DISPLAY
static int wfd_wsd_supported(struct wpabuf *wfd)
{
  const u8 *pos, *end;
  u8 subelem;
  u16 len;

  if (wfd == NULL)
    return 0;

  pos = wpabuf_head(wfd);
  end = pos + wpabuf_len(wfd);

  while (end - pos >= 3) {
    subelem = *pos++;
    len = WPA_GET_BE16(pos);
    pos += 2;
    if (len > end - pos)
      break;

    if (subelem == WFD_SUBELEM_DEVICE_INFO && len >= 6) {
      u16 info = WPA_GET_BE16(pos);
      return !!(info & 0x0040);
    }

    pos += len;
  }

  return 0;
}
#endif /* CONFIG_WIFI_DISPLAY */

struct p2p_sd_query * p2p_pending_sd_req(struct p2p_data *p2p,
           struct p2p_device *dev)
{
  struct p2p_sd_query *q;
  int wsd = 0;
  int count = 0;

  if (!(dev->info.dev_capab & P2P_DEV_CAPAB_SERVICE_DISCOVERY))
    return NULL; /* peer does not support SD */
#ifdef CONFIG_WIFI_DISPLAY
  if (wfd_wsd_supported(dev->info.wfd_subelems))
    wsd = 1;
#endif /* CONFIG_WIFI_DISPLAY */

  for (q = p2p->sd_queries; q; q = q->next) {
    /* Use WSD only if the peer indicates support or it */
    if (q->wsd && !wsd)
      continue;
    /* if the query is a broadcast query */
    if (q->for_all_peers) {
      /*
       * check if there are any broadcast queries pending for
       * this device
       */
      if (dev->sd_pending_bcast_queries <= 0)
        return NULL;
      /* query number that needs to be send to the device */
      if (count == dev->sd_pending_bcast_queries - 1)
        goto found;
      count++;
    }
    if (!q->for_all_peers &&
        ether_addr_equal(q->peer, dev->info.p2p_device_addr))
      goto found;
  }

  return NULL;

found:
  if (dev->sd_reqs > 100) {
    p2p_dbg(p2p, "Too many SD request attempts to " MACSTR
      " - skip remaining queries",
      MAC2STR(dev->info.p2p_device_addr));
    return NULL;
  }
  return q;
}


static void p2p_decrease_sd_bc_queries(struct p2p_data *p2p, int query_number)
{
  struct p2p_device *dev;

  p2p->num_p2p_sd_queries--;
  dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) {
    if (query_number <= dev->sd_pending_bcast_queries - 1) {
      /*
       * Query not yet sent to the device and it is to be
       * removed, so update the pending count.
      */
      dev->sd_pending_bcast_queries--;
    }
  }
}


static int p2p_unlink_sd_query(struct p2p_data *p2p,
             struct p2p_sd_query *query)
{
  struct p2p_sd_query *q, *prev;
  int query_number = 0;

  q = p2p->sd_queries;
  prev = NULL;
  while (q) {
    if (q == query) {
      /* If the query is a broadcast query, decrease one from
       * all the devices */
      if (query->for_all_peers)
        p2p_decrease_sd_bc_queries(p2p, query_number);
      if (prev)
        prev->next = q->next;
      else
        p2p->sd_queries = q->next;
      if (p2p->sd_query == query)
        p2p->sd_query = NULL;
      return 1;
    }
    if (q->for_all_peers)
      query_number++;
    prev = q;
    q = q->next;
  }
  return 0;
}


static void p2p_free_sd_query(struct p2p_sd_query *q)
{
  if (q == NULL)
    return;
  wpabuf_free(q->tlvs);
  os_free(q);
}


void p2p_free_sd_queries(struct p2p_data *p2p)
{
  struct p2p_sd_query *q, *prev;
  q = p2p->sd_queries;
  p2p->sd_queries = NULL;
  while (q) {
    prev = q;
    q = q->next;
    p2p_free_sd_query(prev);
  }
  p2p->num_p2p_sd_queries = 0;
}


static struct wpabuf * p2p_build_sd_query(u16 update_indic,
            struct wpabuf *tlvs)
{
  struct wpabuf *buf;
  u8 *len_pos;

  buf = gas_anqp_build_initial_req(0, 100 + wpabuf_len(tlvs));
  if (buf == NULL)
    return NULL;

  /* ANQP Query Request Frame */
  len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
  wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE);
  wpabuf_put_le16(buf, update_indic); /* Service Update Indicator */
  wpabuf_put_buf(buf, tlvs);
  gas_anqp_set_element_len(buf, len_pos);

  gas_anqp_set_len(buf);

  return buf;
}


static void p2p_send_gas_comeback_req(struct p2p_data *p2p, const u8 *dst,
              u8 dialog_token, int freq)
{
  struct wpabuf *req;

  req = gas_build_comeback_req(dialog_token);
  if (req == NULL)
    return;

  p2p->pending_action_state = P2P_NO_PENDING_ACTION;
  if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr, dst,
          wpabuf_head(req), wpabuf_len(req), 200) < 0)
    p2p_dbg(p2p, "Failed to send Action frame");

  wpabuf_free(req);
}


static struct wpabuf * p2p_build_sd_response(u8 dialog_token, u16 status_code,
               u16 comeback_delay,
               u16 update_indic,
               const struct wpabuf *tlvs)
{
  struct wpabuf *buf;
  u8 *len_pos;

  buf = gas_anqp_build_initial_resp(dialog_token, status_code,
            comeback_delay,
            100 + (tlvs ? wpabuf_len(tlvs) : 0));
  if (buf == NULL)
    return NULL;

  if (tlvs) {
    /* ANQP Query Response Frame */
    len_pos = gas_anqp_add_element(buf, ANQP_VENDOR_SPECIFIC);
    wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE);
     /* Service Update Indicator */
    wpabuf_put_le16(buf, update_indic);
    wpabuf_put_buf(buf, tlvs);
    gas_anqp_set_element_len(buf, len_pos);
  }

  gas_anqp_set_len(buf);

  return buf;
}


static struct wpabuf * p2p_build_gas_comeback_resp(u8 dialog_token,
               u16 status_code,
               u16 update_indic,
               const u8 *data, size_t len,
               u8 frag_id, u8 more,
               u16 total_len)
{
  struct wpabuf *buf;

  buf = gas_anqp_build_comeback_resp(dialog_token, status_code, frag_id,
             more, 0, 100 + len);
  if (buf == NULL)
    return NULL;

  if (frag_id == 0) {
    /* ANQP Query Response Frame */
    wpabuf_put_le16(buf, ANQP_VENDOR_SPECIFIC); /* Info ID */
    wpabuf_put_le16(buf, 3 + 1 + 2 + total_len);
    wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE);
    /* Service Update Indicator */
    wpabuf_put_le16(buf, update_indic);
  }

  wpabuf_put_data(buf, data, len);
  gas_anqp_set_len(buf);

  return buf;
}


int p2p_start_sd(struct p2p_data *p2p, struct p2p_device *dev)
{
  struct wpabuf *req;
  int ret = 0;
  struct p2p_sd_query *query;
  int freq;
  unsigned int wait_time;

  freq = dev->listen_freq > 0 ? dev->listen_freq : dev->oper_freq;
  if (freq <= 0) {
    p2p_dbg(p2p, "No Listen/Operating frequency known for the peer "
      MACSTR " to send SD Request",
      MAC2STR(dev->info.p2p_device_addr));
    return -1;
  }

  query = p2p_pending_sd_req(p2p, dev);
  if (query == NULL)
    return -1;
  if (p2p->state == P2P_SEARCH &&
      ether_addr_equal(p2p->sd_query_no_ack, dev->info.p2p_device_addr)) {
    p2p_dbg(p2p, "Do not start Service Discovery with " MACSTR
      " due to it being the first no-ACK peer in this search iteration",
      MAC2STR(dev->info.p2p_device_addr));
    return -2;
  }

  p2p_dbg(p2p, "Start Service Discovery with " MACSTR,
    MAC2STR(dev->info.p2p_device_addr));

  req = p2p_build_sd_query(p2p->srv_update_indic, query->tlvs);
  if (req == NULL)
    return -1;

  dev->sd_reqs++;
  p2p->sd_peer = dev;
  p2p->sd_query = query;
  p2p->pending_action_state = P2P_PENDING_SD;

  wait_time = 5000;
  if (p2p->cfg->max_listen && wait_time > p2p->cfg->max_listen)
    wait_time = p2p->cfg->max_listen;
  if (p2p_send_action(p2p, freq, dev->info.p2p_device_addr,
          p2p->cfg->dev_addr, dev->info.p2p_device_addr,
          wpabuf_head(req), wpabuf_len(req), wait_time) < 0) {
    p2p_dbg(p2p, "Failed to send Action frame");
    ret = -1;
  }

  wpabuf_free(req);

  return ret;
}


void p2p_rx_gas_initial_req(struct p2p_data *p2p, const u8 *sa,
          const u8 *data, size_t len, int rx_freq)
{
  const u8 *pos = data;
  const u8 *end = data + len;
  const u8 *next;
  u8 dialog_token;
  u16 slen;
  int freq;
  u16 update_indic;


  if (p2p->cfg->sd_request == NULL)
    return;

  if (rx_freq > 0)
    freq = rx_freq;
  else
    freq = p2p_channel_to_freq(p2p->cfg->reg_class,
             p2p->cfg->channel);
  if (freq < 0)
    return;

  if (len < 1 + 2)
    return;

  dialog_token = *pos++;
  p2p_dbg(p2p, "GAS Initial Request from " MACSTR
    " (dialog token %u, freq %d)",
    MAC2STR(sa), dialog_token, rx_freq);

  if (*pos != WLAN_EID_ADV_PROTO) {
    p2p_dbg(p2p, "Unexpected IE in GAS Initial Request: %u", *pos);
    return;
  }
  pos++;

  slen = *pos++;
  if (slen > end - pos || slen < 2) {
    p2p_dbg(p2p, "Invalid IE in GAS Initial Request");
    return;
  }
  next = pos + slen;
  pos++; /* skip QueryRespLenLimit and PAME-BI */

  if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
    p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
      *pos);
    return;
  }

  pos = next;
  /* Query Request */
  if (end - pos < 2)
    return;
  slen = WPA_GET_LE16(pos);
  pos += 2;
  if (slen > end - pos)
    return;
  end = pos + slen;

  /* ANQP Query Request */
  if (end - pos < 4)
    return;
  if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
    p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
    return;
  }
  pos += 2;

  slen = WPA_GET_LE16(pos);
  pos += 2;
  if (slen > end - pos || slen < 3 + 1) {
    p2p_dbg(p2p, "Invalid ANQP Query Request length");
    return;
  }

  if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
    p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
      WPA_GET_BE32(pos));
    return;
  }
  pos += 4;

  if (end - pos < 2)
    return;
  update_indic = WPA_GET_LE16(pos);
  p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
  pos += 2;

  p2p->cfg->sd_request(p2p->cfg->cb_ctx, freq, sa, dialog_token,
           update_indic, pos, end - pos);
  /* the response will be indicated with a call to p2p_sd_response() */
}


void p2p_sd_response(struct p2p_data *p2p, int freq, const u8 *dst,
         u8 dialog_token, const struct wpabuf *resp_tlvs)
{
  struct wpabuf *resp;
  size_t max_len;
  unsigned int wait_time = 200;

  /*
   * In the 60 GHz, we have a smaller maximum frame length for management
   * frames.
   */
  max_len = (freq > 56160) ? 928 : 1400;

  /* TODO: fix the length limit to match with the maximum frame length */
  if (wpabuf_len(resp_tlvs) > max_len) {
    p2p_dbg(p2p, "SD response long enough to require fragmentation");
    if (p2p->sd_resp) {
      /*
       * TODO: Could consider storing the fragmented response
       * separately for each peer to avoid having to drop old
       * one if there is more than one pending SD query.
       * Though, that would eat more memory, so there are
       * also benefits to just using a single buffer.
       */
      p2p_dbg(p2p, "Drop previous SD response");
      wpabuf_free(p2p->sd_resp);
    }
    p2p->sd_resp = wpabuf_dup(resp_tlvs);
    if (p2p->sd_resp == NULL) {
      p2p_err(p2p, "Failed to allocate SD response fragmentation area");
      return;
    }
    os_memcpy(p2p->sd_resp_addr, dst, ETH_ALEN);
    p2p->sd_resp_dialog_token = dialog_token;
    p2p->sd_resp_pos = 0;
    p2p->sd_frag_id = 0;
    resp = p2p_build_sd_response(dialog_token, WLAN_STATUS_SUCCESS,
               1, p2p->srv_update_indic, NULL);
  } else {
    p2p_dbg(p2p, "SD response fits in initial response");
    wait_time = 0; /* no more SD frames in the sequence */
    resp = p2p_build_sd_response(dialog_token,
               WLAN_STATUS_SUCCESS, 0,
               p2p->srv_update_indic, resp_tlvs);
  }
  if (resp == NULL)
    return;

  p2p->pending_action_state = P2P_NO_PENDING_ACTION;
  if (p2p_send_action(p2p, freq, dst, p2p->cfg->dev_addr,
          p2p->cfg->dev_addr,
          wpabuf_head(resp), wpabuf_len(resp), wait_time) < 0)
    p2p_dbg(p2p, "Failed to send Action frame");

  wpabuf_free(resp);
}


void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
           const u8 *data, size_t len, int rx_freq)
{
  const u8 *pos = data;
  const u8 *end = data + len;
  const u8 *next;
  u8 dialog_token;
  u16 status_code;
  u16 comeback_delay;
  u16 slen;
  u16 update_indic;

  if ((p2p->state != P2P_SD_DURING_FIND && p2p->state != P2P_SEARCH) ||
      !p2p->sd_peer ||
      !ether_addr_equal(sa, p2p->sd_peer->info.p2p_device_addr)) {
    p2p_dbg(p2p, "Ignore unexpected GAS Initial Response from "
      MACSTR, MAC2STR(sa));
    return;
  }
  if (p2p->state == P2P_SEARCH) {
    /* It is possible for the TX status and RX response events to be
     * reordered, so assume the request was ACKed if a response is
     * received. */
    p2p_dbg(p2p,
      "GAS Initial Request had not yet received TX status - process the response anyway");
    p2p_sd_query_cb(p2p, 1);
  }
  p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
  p2p_clear_timeout(p2p);

  p2p_dbg(p2p, "Received GAS Initial Response from " MACSTR " (len=%d)",
    MAC2STR(sa), (int) len);

  if (len < 5 + 2) {
    p2p_dbg(p2p, "Too short GAS Initial Response frame");
    return;
  }

  dialog_token = *pos++;
  /* TODO: check dialog_token match */
  status_code = WPA_GET_LE16(pos);
  pos += 2;
  comeback_delay = WPA_GET_LE16(pos);
  pos += 2;
  p2p_dbg(p2p, "dialog_token=%u status_code=%u comeback_delay=%u",
    dialog_token, status_code, comeback_delay);
  if (status_code) {
    p2p_dbg(p2p, "Service Discovery failed: status code %u",
      status_code);
    return;
  }

  if (*pos != WLAN_EID_ADV_PROTO) {
    p2p_dbg(p2p, "Unexpected IE in GAS Initial Response: %u", *pos);
    return;
  }
  pos++;

  slen = *pos++;
  if (slen > end - pos || slen < 2) {
    p2p_dbg(p2p, "Invalid IE in GAS Initial Response");
    return;
  }
  next = pos + slen;
  pos++; /* skip QueryRespLenLimit and PAME-BI */

  if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
    p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
      *pos);
    return;
  }

  pos = next;
  /* Query Response */
  if (end - pos < 2) {
    p2p_dbg(p2p, "Too short Query Response");
    return;
  }
  slen = WPA_GET_LE16(pos);
  pos += 2;
  p2p_dbg(p2p, "Query Response Length: %d", slen);
  if (slen > end - pos) {
    p2p_dbg(p2p, "Not enough Query Response data");
    return;
  }
  end = pos + slen;

  if (comeback_delay) {
    p2p_dbg(p2p, "Fragmented response - request fragments");
    if (p2p->sd_rx_resp) {
      p2p_dbg(p2p, "Drop old SD reassembly buffer");
      wpabuf_free(p2p->sd_rx_resp);
      p2p->sd_rx_resp = NULL;
    }
    p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
    return;
  }

  /* ANQP Query Response */
  if (end - pos < 4)
    return;
  if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
    p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
    return;
  }
  pos += 2;

  slen = WPA_GET_LE16(pos);
  pos += 2;
  if (slen > end - pos || slen < 3 + 1) {
    p2p_dbg(p2p, "Invalid ANQP Query Response length");
    return;
  }

  if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
    p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
      WPA_GET_BE32(pos));
    return;
  }
  pos += 4;

  if (end - pos < 2)
    return;
  update_indic = WPA_GET_LE16(pos);
  p2p_dbg(p2p, "Service Update Indicator: %u", update_indic);
  pos += 2;

  p2p->sd_peer = NULL;

  if (p2p->sd_query) {
    if (!p2p->sd_query->for_all_peers) {
      struct p2p_sd_query *q;
      p2p_dbg(p2p, "Remove completed SD query %p",
        p2p->sd_query);
      q = p2p->sd_query;
      p2p_unlink_sd_query(p2p, p2p->sd_query);
      p2p_free_sd_query(q);
    }
    p2p->sd_query = NULL;
  }

  if (p2p->cfg->sd_response)
    p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa, update_indic,
              pos, end - pos);
  p2p_continue_find(p2p);
}


void p2p_rx_gas_comeback_req(struct p2p_data *p2p, const u8 *sa,
           const u8 *data, size_t len, int rx_freq)
{
  struct wpabuf *resp;
  u8 dialog_token;
  size_t frag_len, max_len;
  int more = 0;
  unsigned int wait_time = 200;

  wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Request", data, len);
  if (len < 1)
    return;
  dialog_token = *data;
  p2p_dbg(p2p, "Dialog Token: %u", dialog_token);
  if (dialog_token != p2p->sd_resp_dialog_token) {
    p2p_dbg(p2p, "No pending SD response fragment for dialog token %u",
      dialog_token);
    return;
  }

  if (p2p->sd_resp == NULL) {
    p2p_dbg(p2p, "No pending SD response fragment available");
    return;
  }
  if (!ether_addr_equal(sa, p2p->sd_resp_addr)) {
    p2p_dbg(p2p, "No pending SD response fragment for " MACSTR,
      MAC2STR(sa));
    return;
  }

  /*
   * In the 60 GHz, we have a smaller maximum frame length for management
   * frames.
   */
  max_len = (rx_freq > 56160) ? 928 : 1400;
  frag_len = wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos;
  if (frag_len > max_len) {
    frag_len = max_len;
    more = 1;
  }
  resp = p2p_build_gas_comeback_resp(dialog_token, WLAN_STATUS_SUCCESS,
             p2p->srv_update_indic,
             wpabuf_head_u8(p2p->sd_resp) +
             p2p->sd_resp_pos, frag_len,
             p2p->sd_frag_id, more,
             wpabuf_len(p2p->sd_resp));
  if (resp == NULL)
    return;
  p2p_dbg(p2p, "Send GAS Comeback Response (frag_id %d more=%d frag_len=%d)",
    p2p->sd_frag_id, more, (int) frag_len);
  p2p->sd_frag_id++;
  p2p->sd_resp_pos += frag_len;

  if (more) {
    p2p_dbg(p2p, "%d more bytes remain to be sent",
      (int) (wpabuf_len(p2p->sd_resp) - p2p->sd_resp_pos));
  } else {
    p2p_dbg(p2p, "All fragments of SD response sent");
    wpabuf_free(p2p->sd_resp);
    p2p->sd_resp = NULL;
    wait_time = 0; /* no more SD frames in the sequence */
  }

  p2p->pending_action_state = P2P_NO_PENDING_ACTION;
  if (p2p_send_action(p2p, rx_freq, sa, p2p->cfg->dev_addr,
          p2p->cfg->dev_addr,
          wpabuf_head(resp), wpabuf_len(resp), wait_time) < 0)
    p2p_dbg(p2p, "Failed to send Action frame");

  wpabuf_free(resp);
}


void p2p_rx_gas_comeback_resp(struct p2p_data *p2p, const u8 *sa,
            const u8 *data, size_t len, int rx_freq)
{
  const u8 *pos = data;
  const u8 *end = data + len;
  const u8 *next;
  u8 dialog_token;
  u16 status_code;
  u8 frag_id;
  u8 more_frags;
  u16 comeback_delay;
  u16 slen;

  wpa_hexdump(MSG_DEBUG, "P2P: RX GAS Comeback Response", data, len);

  if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
      !ether_addr_equal(sa, p2p->sd_peer->info.p2p_device_addr)) {
    p2p_dbg(p2p, "Ignore unexpected GAS Comeback Response from "
      MACSTR, MAC2STR(sa));
    return;
  }
  p2p->cfg->send_action_done(p2p->cfg->cb_ctx);
  p2p_clear_timeout(p2p);

  p2p_dbg(p2p, "Received GAS Comeback Response from " MACSTR " (len=%d)",
    MAC2STR(sa), (int) len);

  if (len < 6 + 2) {
    p2p_dbg(p2p, "Too short GAS Comeback Response frame");
    return;
  }

  dialog_token = *pos++;
  /* TODO: check dialog_token match */
  status_code = WPA_GET_LE16(pos);
  pos += 2;
  frag_id = *pos & 0x7f;
  more_frags = (*pos & 0x80) >> 7;
  pos++;
  comeback_delay = WPA_GET_LE16(pos);
  pos += 2;
  p2p_dbg(p2p, "dialog_token=%u status_code=%u frag_id=%d more_frags=%d "
    "comeback_delay=%u",
    dialog_token, status_code, frag_id, more_frags,
    comeback_delay);
  /* TODO: check frag_id match */
  if (status_code) {
    p2p_dbg(p2p, "Service Discovery failed: status code %u",
      status_code);
    return;
  }

  if (*pos != WLAN_EID_ADV_PROTO) {
    p2p_dbg(p2p, "Unexpected IE in GAS Comeback Response: %u",
      *pos);
    return;
  }
  pos++;

  slen = *pos++;
  if (slen > end - pos || slen < 2) {
    p2p_dbg(p2p, "Invalid IE in GAS Comeback Response");
    return;
  }
  next = pos + slen;
  pos++; /* skip QueryRespLenLimit and PAME-BI */

  if (*pos != ACCESS_NETWORK_QUERY_PROTOCOL) {
    p2p_dbg(p2p, "Unsupported GAS advertisement protocol id %u",
      *pos);
    return;
  }

  pos = next;
  /* Query Response */
  if (end - pos < 2) {
    p2p_dbg(p2p, "Too short Query Response");
    return;
  }
  slen = WPA_GET_LE16(pos);
  pos += 2;
  p2p_dbg(p2p, "Query Response Length: %d", slen);
  if (slen > end - pos) {
    p2p_dbg(p2p, "Not enough Query Response data");
    return;
  }
  if (slen == 0) {
    p2p_dbg(p2p, "No Query Response data");
    return;
  }
  end = pos + slen;

  if (p2p->sd_rx_resp) {
     /*
      * ANQP header is only included in the first fragment; rest of
      * the fragments start with continue TLVs.
      */
    goto skip_nqp_header;
  }

  /* ANQP Query Response */
  if (end - pos < 4)
    return;
  if (WPA_GET_LE16(pos) != ANQP_VENDOR_SPECIFIC) {
    p2p_dbg(p2p, "Unsupported ANQP Info ID %u", WPA_GET_LE16(pos));
    return;
  }
  pos += 2;

  slen = WPA_GET_LE16(pos);
  pos += 2;
  p2p_dbg(p2p, "ANQP Query Response length: %u", slen);
  if (slen < 3 + 1) {
    p2p_dbg(p2p, "Invalid ANQP Query Response length");
    return;
  }
  if (end - pos < 4)
    return;

  if (WPA_GET_BE32(pos) != P2P_IE_VENDOR_TYPE) {
    p2p_dbg(p2p, "Unsupported ANQP vendor OUI-type %08x",
      WPA_GET_BE32(pos));
    return;
  }
  pos += 4;

  if (end - pos < 2)
    return;
  p2p->sd_rx_update_indic = WPA_GET_LE16(pos);
  p2p_dbg(p2p, "Service Update Indicator: %u", p2p->sd_rx_update_indic);
  pos += 2;

skip_nqp_header:
  if (wpabuf_resize(&p2p->sd_rx_resp, end - pos) < 0)
    return;
  wpabuf_put_data(p2p->sd_rx_resp, pos, end - pos);
  p2p_dbg(p2p, "Current SD reassembly buffer length: %u",
    (unsigned int) wpabuf_len(p2p->sd_rx_resp));

  if (more_frags) {
    p2p_dbg(p2p, "More fragments remains");
    /* TODO: what would be a good size limit? */
    if (wpabuf_len(p2p->sd_rx_resp) > 64000) {
      wpabuf_free(p2p->sd_rx_resp);
      p2p->sd_rx_resp = NULL;
      p2p_dbg(p2p, "Too long SD response - drop it");
      return;
    }
    p2p_send_gas_comeback_req(p2p, sa, dialog_token, rx_freq);
    return;
  }

  p2p->sd_peer = NULL;

  if (p2p->sd_query) {
    if (!p2p->sd_query->for_all_peers) {
      struct p2p_sd_query *q;
      p2p_dbg(p2p, "Remove completed SD query %p",
        p2p->sd_query);
      q = p2p->sd_query;
      p2p_unlink_sd_query(p2p, p2p->sd_query);
      p2p_free_sd_query(q);
    }
    p2p->sd_query = NULL;
  }

  if (p2p->cfg->sd_response)
    p2p->cfg->sd_response(p2p->cfg->cb_ctx, sa,
              p2p->sd_rx_update_indic,
              wpabuf_head(p2p->sd_rx_resp),
              wpabuf_len(p2p->sd_rx_resp));
  wpabuf_free(p2p->sd_rx_resp);
  p2p->sd_rx_resp = NULL;

  p2p_continue_find(p2p);
}


void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
          const struct wpabuf *tlvs)
{
  struct p2p_sd_query *q;

  q = os_zalloc(sizeof(*q));
  if (q == NULL)
    return NULL;

  if (dst)
    os_memcpy(q->peer, dst, ETH_ALEN);
  else
    q->for_all_peers = 1;

  q->tlvs = wpabuf_dup(tlvs);
  if (q->tlvs == NULL) {
    p2p_free_sd_query(q);
    return NULL;
  }

  q->next = p2p->sd_queries;
  p2p->sd_queries = q;
  p2p_dbg(p2p, "Added SD Query %p", q);

  if (dst == NULL) {
    struct p2p_device *dev;

    p2p->num_p2p_sd_queries++;

    /* Update all the devices for the newly added broadcast query */
    dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) {
      if (dev->sd_pending_bcast_queries <= 0)
        dev->sd_pending_bcast_queries = 1;
      else
        dev->sd_pending_bcast_queries++;
    }
  }

  return q;
}


#ifdef CONFIG_WIFI_DISPLAY
void * p2p_sd_request_wfd(struct p2p_data *p2p, const u8 *dst,
        const struct wpabuf *tlvs)
{
  struct p2p_sd_query *q;
  q = p2p_sd_request(p2p, dst, tlvs);
  if (q)
    q->wsd = 1;
  return q;
}
#endif /* CONFIG_WIFI_DISPLAY */


void p2p_sd_service_update(struct p2p_data *p2p)
{
  p2p->srv_update_indic++;
}


int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
{
  if (p2p_unlink_sd_query(p2p, req)) {
    p2p_dbg(p2p, "Cancel pending SD query %p", req);
    p2p_free_sd_query(req);
    return 0;
  }
  return -1;
}

Coverage Report

Created: 2025-08-26 06:04