libeap/src/p2p/p2p_build.c

   1 /*
   2  * P2P - IE builder
   3  * Copyright (c) 2009-2010, 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/qca-vendor.h"
  14 #include "wps/wps_i.h"
  15 #include "p2p_i.h"
  16
  17
  18 void p2p_buf_add_action_hdr(struct wpabuf *buf, u8 subtype, u8 dialog_token)
  19 {
  20         wpabuf_put_u8(buf, WLAN_ACTION_VENDOR_SPECIFIC);
  21         wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE);
  22
  23         wpabuf_put_u8(buf, subtype); /* OUI Subtype */
  24         wpabuf_put_u8(buf, dialog_token);
  25         wpa_printf(MSG_DEBUG, "P2P: * Dialog Token: %d", dialog_token);
  26 }
  27
  28
  29 void p2p_buf_add_public_action_hdr(struct wpabuf *buf, u8 subtype,
  30                                    u8 dialog_token)
  31 {
  32         wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC);
  33         wpabuf_put_u8(buf, WLAN_PA_VENDOR_SPECIFIC);
  34         wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE);
  35
  36         wpabuf_put_u8(buf, subtype); /* OUI Subtype */
  37         wpabuf_put_u8(buf, dialog_token);
  38         wpa_printf(MSG_DEBUG, "P2P: * Dialog Token: %d", dialog_token);
  39 }
  40
  41
  42 u8 * p2p_buf_add_ie_hdr(struct wpabuf *buf)
  43 {
  44         u8 *len;
  45
  46         /* P2P IE header */
  47         wpabuf_put_u8(buf, WLAN_EID_VENDOR_SPECIFIC);
  48         len = wpabuf_put(buf, 1); /* IE length to be filled */
  49         wpabuf_put_be32(buf, P2P_IE_VENDOR_TYPE);
  50         wpa_printf(MSG_DEBUG, "P2P: * P2P IE header");
  51         return len;
  52 }
  53
  54
  55 void p2p_buf_update_ie_hdr(struct wpabuf *buf, u8 *len)
  56 {
  57         /* Update P2P IE Length */
  58         *len = (u8 *) wpabuf_put(buf, 0) - len - 1;
  59 }
  60
  61
  62 void p2p_buf_add_capability(struct wpabuf *buf, u8 dev_capab, u8 group_capab)
  63 {
  64         /* P2P Capability */
  65         wpabuf_put_u8(buf, P2P_ATTR_CAPABILITY);
  66         wpabuf_put_le16(buf, 2);
  67         wpabuf_put_u8(buf, dev_capab); /* Device Capabilities */
  68         wpabuf_put_u8(buf, group_capab); /* Group Capabilities */
  69         wpa_printf(MSG_DEBUG, "P2P: * Capability dev=%02x group=%02x",
  70                    dev_capab, group_capab);
  71 }
  72
  73
  74 void p2p_buf_add_go_intent(struct wpabuf *buf, u8 go_intent)
  75 {
  76         /* Group Owner Intent */
  77         wpabuf_put_u8(buf, P2P_ATTR_GROUP_OWNER_INTENT);
  78         wpabuf_put_le16(buf, 1);
  79         wpabuf_put_u8(buf, go_intent);
  80         wpa_printf(MSG_DEBUG, "P2P: * GO Intent: Intent %u Tie breaker %u",
  81                    go_intent >> 1, go_intent & 0x01);
  82 }
  83
  84
  85 void p2p_buf_add_listen_channel(struct wpabuf *buf, const char *country,
  86                                 u8 reg_class, u8 channel)
  87 {
  88         /* Listen Channel */
  89         wpabuf_put_u8(buf, P2P_ATTR_LISTEN_CHANNEL);
  90         wpabuf_put_le16(buf, 5);
  91         wpabuf_put_data(buf, country, 3);
  92         wpabuf_put_u8(buf, reg_class); /* Regulatory Class */
  93         wpabuf_put_u8(buf, channel); /* Channel Number */
  94         wpa_printf(MSG_DEBUG, "P2P: * Listen Channel: Regulatory Class %u "
  95                    "Channel %u", reg_class, channel);
  96 }
  97
  98
  99 void p2p_buf_add_operating_channel(struct wpabuf *buf, const char *country,
 100                                    u8 reg_class, u8 channel)
 101 {
 102         /* Operating Channel */
 103         wpabuf_put_u8(buf, P2P_ATTR_OPERATING_CHANNEL);
 104         wpabuf_put_le16(buf, 5);
 105         wpabuf_put_data(buf, country, 3);
 106         wpabuf_put_u8(buf, reg_class); /* Regulatory Class */
 107         wpabuf_put_u8(buf, channel); /* Channel Number */
 108         wpa_printf(MSG_DEBUG, "P2P: * Operating Channel: Regulatory Class %u "
 109                    "Channel %u", reg_class, channel);
 110 }
 111
 112
 113 void p2p_buf_add_pref_channel_list(struct wpabuf *buf,
 114                                    const u32 *preferred_freq_list,
 115                                    unsigned int size)
 116 {
 117         unsigned int i, count = 0;
 118         u8 op_class, op_channel;
 119
 120         if (!size)
 121                 return;
 122
 123         /*
 124          * First, determine the number of P2P supported channels in the
 125          * pref_freq_list returned from driver. This is needed for calculations
 126          * of the vendor IE size.
 127          */
 128         for (i = 0; i < size; i++) {
 129                 if (p2p_freq_to_channel(preferred_freq_list[i], &op_class,
 130                                         &op_channel) == 0)
 131                         count++;
 132         }
 133
 134         wpabuf_put_u8(buf, WLAN_EID_VENDOR_SPECIFIC);
 135         wpabuf_put_u8(buf, 4 + count * sizeof(u16));
 136         wpabuf_put_be24(buf, OUI_QCA);
 137         wpabuf_put_u8(buf, QCA_VENDOR_ELEM_P2P_PREF_CHAN_LIST);
 138         for (i = 0; i < size; i++) {
 139                 if (p2p_freq_to_channel(preferred_freq_list[i], &op_class,
 140                                         &op_channel) < 0) {
 141                         wpa_printf(MSG_DEBUG, "Unsupported frequency %u MHz",
 142                                    preferred_freq_list[i]);
 143                         continue;
 144                 }
 145                 wpabuf_put_u8(buf, op_class);
 146                 wpabuf_put_u8(buf, op_channel);
 147         }
 148 }
 149
 150
 151 void p2p_buf_add_channel_list(struct wpabuf *buf, const char *country,
 152                               struct p2p_channels *chan)
 153 {
 154         u8 *len;
 155         size_t i;
 156
 157         /* Channel List */
 158         wpabuf_put_u8(buf, P2P_ATTR_CHANNEL_LIST);
 159         len = wpabuf_put(buf, 2); /* IE length to be filled */
 160         wpabuf_put_data(buf, country, 3); /* Country String */
 161
 162         for (i = 0; i < chan->reg_classes; i++) {
 163                 struct p2p_reg_class *c = &chan->reg_class[i];
 164                 wpabuf_put_u8(buf, c->reg_class);
 165                 wpabuf_put_u8(buf, c->channels);
 166                 wpabuf_put_data(buf, c->channel, c->channels);
 167         }
 168
 169         /* Update attribute length */
 170         WPA_PUT_LE16(len, (u8 *) wpabuf_put(buf, 0) - len - 2);
 171         wpa_hexdump(MSG_DEBUG, "P2P: * Channel List",
 172                     len + 2, (u8 *) wpabuf_put(buf, 0) - len - 2);
 173 }
 174
 175
 176 void p2p_buf_add_status(struct wpabuf *buf, u8 status)
 177 {
 178         /* Status */
 179         wpabuf_put_u8(buf, P2P_ATTR_STATUS);
 180         wpabuf_put_le16(buf, 1);
 181         wpabuf_put_u8(buf, status);
 182         wpa_printf(MSG_DEBUG, "P2P: * Status: %d", status);
 183 }
 184
 185
 186 void p2p_buf_add_device_info(struct wpabuf *buf, struct p2p_data *p2p,
 187                              struct p2p_device *peer)
 188 {
 189         u8 *len;
 190         u16 methods;
 191         size_t nlen, i;
 192
 193         /* P2P Device Info */
 194         wpabuf_put_u8(buf, P2P_ATTR_DEVICE_INFO);
 195         len = wpabuf_put(buf, 2); /* IE length to be filled */
 196
 197         /* P2P Device address */
 198         wpabuf_put_data(buf, p2p->cfg->dev_addr, ETH_ALEN);
 199
 200         /* Config Methods */
 201         methods = 0;
 202         if (peer && peer->wps_method != WPS_NOT_READY) {
 203                 if (peer->wps_method == WPS_PBC)
 204                         methods |= WPS_CONFIG_PUSHBUTTON;
 205                 else if (peer->wps_method == WPS_PIN_DISPLAY ||
 206                          peer->wps_method == WPS_PIN_KEYPAD) {
 207                         methods |= WPS_CONFIG_DISPLAY | WPS_CONFIG_KEYPAD;
 208                         methods |= WPS_CONFIG_P2PS;
 209                 }
 210         } else if (p2p->cfg->config_methods) {
 211                 methods |= p2p->cfg->config_methods &
 212                         (WPS_CONFIG_PUSHBUTTON | WPS_CONFIG_DISPLAY |
 213                          WPS_CONFIG_KEYPAD | WPS_CONFIG_P2PS);
 214         } else {
 215                 methods |= WPS_CONFIG_PUSHBUTTON;
 216                 methods |= WPS_CONFIG_DISPLAY | WPS_CONFIG_KEYPAD;
 217                 methods |= WPS_CONFIG_P2PS;
 218         }
 219         wpabuf_put_be16(buf, methods);
 220
 221         /* Primary Device Type */
 222         wpabuf_put_data(buf, p2p->cfg->pri_dev_type,
 223                         sizeof(p2p->cfg->pri_dev_type));
 224
 225         /* Number of Secondary Device Types */
 226         wpabuf_put_u8(buf, p2p->cfg->num_sec_dev_types);
 227
 228         /* Secondary Device Type List */
 229         for (i = 0; i < p2p->cfg->num_sec_dev_types; i++)
 230                 wpabuf_put_data(buf, p2p->cfg->sec_dev_type[i],
 231                                 WPS_DEV_TYPE_LEN);
 232
 233         /* Device Name */
 234         nlen = p2p->cfg->dev_name ? os_strlen(p2p->cfg->dev_name) : 0;
 235         wpabuf_put_be16(buf, ATTR_DEV_NAME);
 236         wpabuf_put_be16(buf, nlen);
 237         wpabuf_put_data(buf, p2p->cfg->dev_name, nlen);
 238
 239         /* Update attribute length */
 240         WPA_PUT_LE16(len, (u8 *) wpabuf_put(buf, 0) - len - 2);
 241         wpa_printf(MSG_DEBUG, "P2P: * Device Info");
 242 }
 243
 244
 245 void p2p_buf_add_device_id(struct wpabuf *buf, const u8 *dev_addr)
 246 {
 247         /* P2P Device ID */
 248         wpabuf_put_u8(buf, P2P_ATTR_DEVICE_ID);
 249         wpabuf_put_le16(buf, ETH_ALEN);
 250         wpabuf_put_data(buf, dev_addr, ETH_ALEN);
 251         wpa_printf(MSG_DEBUG, "P2P: * Device ID: " MACSTR, MAC2STR(dev_addr));
 252 }
 253
 254
 255 void p2p_buf_add_config_timeout(struct wpabuf *buf, u8 go_timeout,
 256                                 u8 client_timeout)
 257 {
 258         /* Configuration Timeout */
 259         wpabuf_put_u8(buf, P2P_ATTR_CONFIGURATION_TIMEOUT);
 260         wpabuf_put_le16(buf, 2);
 261         wpabuf_put_u8(buf, go_timeout);
 262         wpabuf_put_u8(buf, client_timeout);
 263         wpa_printf(MSG_DEBUG, "P2P: * Configuration Timeout: GO %d (*10ms)  "
 264                    "client %d (*10ms)", go_timeout, client_timeout);
 265 }
 266
 267
 268 void p2p_buf_add_intended_addr(struct wpabuf *buf, const u8 *interface_addr)
 269 {
 270         /* Intended P2P Interface Address */
 271         wpabuf_put_u8(buf, P2P_ATTR_INTENDED_INTERFACE_ADDR);
 272         wpabuf_put_le16(buf, ETH_ALEN);
 273         wpabuf_put_data(buf, interface_addr, ETH_ALEN);
 274         wpa_printf(MSG_DEBUG, "P2P: * Intended P2P Interface Address " MACSTR,
 275                    MAC2STR(interface_addr));
 276 }
 277
 278
 279 void p2p_buf_add_group_bssid(struct wpabuf *buf, const u8 *bssid)
 280 {
 281         /* P2P Group BSSID */
 282         wpabuf_put_u8(buf, P2P_ATTR_GROUP_BSSID);
 283         wpabuf_put_le16(buf, ETH_ALEN);
 284         wpabuf_put_data(buf, bssid, ETH_ALEN);
 285         wpa_printf(MSG_DEBUG, "P2P: * P2P Group BSSID " MACSTR,
 286                    MAC2STR(bssid));
 287 }
 288
 289
 290 void p2p_buf_add_group_id(struct wpabuf *buf, const u8 *dev_addr,
 291                           const u8 *ssid, size_t ssid_len)
 292 {
 293         /* P2P Group ID */
 294         wpabuf_put_u8(buf, P2P_ATTR_GROUP_ID);
 295         wpabuf_put_le16(buf, ETH_ALEN + ssid_len);
 296         wpabuf_put_data(buf, dev_addr, ETH_ALEN);
 297         wpabuf_put_data(buf, ssid, ssid_len);
 298         wpa_printf(MSG_DEBUG, "P2P: * P2P Group ID " MACSTR,
 299                    MAC2STR(dev_addr));
 300         wpa_hexdump_ascii(MSG_DEBUG, "P2P: P2P Group ID SSID", ssid, ssid_len);
 301 }
 302
 303
 304 void p2p_buf_add_invitation_flags(struct wpabuf *buf, u8 flags)
 305 {
 306         /* Invitation Flags */
 307         wpabuf_put_u8(buf, P2P_ATTR_INVITATION_FLAGS);
 308         wpabuf_put_le16(buf, 1);
 309         wpabuf_put_u8(buf, flags);
 310         wpa_printf(MSG_DEBUG, "P2P: * Invitation Flags: bitmap 0x%x", flags);
 311 }
 312
 313
 314 static void p2p_buf_add_noa_desc(struct wpabuf *buf, struct p2p_noa_desc *desc)
 315 {
 316         if (desc == NULL)
 317                 return;
 318
 319         wpabuf_put_u8(buf, desc->count_type);
 320         wpabuf_put_le32(buf, desc->duration);
 321         wpabuf_put_le32(buf, desc->interval);
 322         wpabuf_put_le32(buf, desc->start_time);
 323 }
 324
 325
 326 void p2p_buf_add_noa(struct wpabuf *buf, u8 noa_index, u8 opp_ps, u8 ctwindow,
 327                      struct p2p_noa_desc *desc1, struct p2p_noa_desc *desc2)
 328 {
 329         /* Notice of Absence */
 330         wpabuf_put_u8(buf, P2P_ATTR_NOTICE_OF_ABSENCE);
 331         wpabuf_put_le16(buf, 2 + (desc1 ? 13 : 0) + (desc2 ? 13 : 0));
 332         wpabuf_put_u8(buf, noa_index);
 333         wpabuf_put_u8(buf, (opp_ps ? 0x80 : 0) | (ctwindow & 0x7f));
 334         p2p_buf_add_noa_desc(buf, desc1);
 335         p2p_buf_add_noa_desc(buf, desc2);
 336         wpa_printf(MSG_DEBUG, "P2P: * Notice of Absence");
 337 }
 338
 339
 340 void p2p_buf_add_ext_listen_timing(struct wpabuf *buf, u16 period,
 341                                    u16 interval)
 342 {
 343         /* Extended Listen Timing */
 344         wpabuf_put_u8(buf, P2P_ATTR_EXT_LISTEN_TIMING);
 345         wpabuf_put_le16(buf, 4);
 346         wpabuf_put_le16(buf, period);
 347         wpabuf_put_le16(buf, interval);
 348         wpa_printf(MSG_DEBUG, "P2P: * Extended Listen Timing (period %u msec  "
 349                    "interval %u msec)", period, interval);
 350 }
 351
 352
 353 void p2p_buf_add_p2p_interface(struct wpabuf *buf, struct p2p_data *p2p)
 354 {
 355         /* P2P Interface */
 356         wpabuf_put_u8(buf, P2P_ATTR_INTERFACE);
 357         wpabuf_put_le16(buf, ETH_ALEN + 1 + ETH_ALEN);
 358         /* P2P Device address */
 359         wpabuf_put_data(buf, p2p->cfg->dev_addr, ETH_ALEN);
 360         /*
 361          * FIX: Fetch interface address list from driver. Do not include
 362          * the P2P Device address if it is never used as interface address.
 363          */
 364         /* P2P Interface Address Count */
 365         wpabuf_put_u8(buf, 1);
 366         wpabuf_put_data(buf, p2p->cfg->dev_addr, ETH_ALEN);
 367 }
 368
 369
 370 void p2p_buf_add_oob_go_neg_channel(struct wpabuf *buf, const char *country,
 371                                     u8 oper_class, u8 channel,
 372                                     enum p2p_role_indication role)
 373 {
 374         /* OOB Group Owner Negotiation Channel */
 375         wpabuf_put_u8(buf, P2P_ATTR_OOB_GO_NEG_CHANNEL);
 376         wpabuf_put_le16(buf, 6);
 377         wpabuf_put_data(buf, country, 3);
 378         wpabuf_put_u8(buf, oper_class); /* Operating Class */
 379         wpabuf_put_u8(buf, channel); /* Channel Number */
 380         wpabuf_put_u8(buf, (u8) role); /* Role indication */
 381         wpa_printf(MSG_DEBUG, "P2P: * OOB GO Negotiation Channel: Operating "
 382                    "Class %u Channel %u Role %d",
 383                    oper_class, channel, role);
 384 }
 385
 386
 387 void p2p_buf_add_service_hash(struct wpabuf *buf, struct p2p_data *p2p)
 388 {
 389         if (!p2p)
 390                 return;
 391
 392         /* Service Hash */
 393         wpabuf_put_u8(buf, P2P_ATTR_SERVICE_HASH);
 394         wpabuf_put_le16(buf, p2p->p2ps_seek_count * P2PS_HASH_LEN);
 395         wpabuf_put_data(buf, p2p->p2ps_seek_hash,
 396                         p2p->p2ps_seek_count * P2PS_HASH_LEN);
 397         wpa_hexdump(MSG_DEBUG, "P2P: * Service Hash",
 398                     p2p->p2ps_seek_hash, p2p->p2ps_seek_count * P2PS_HASH_LEN);
 399 }
 400
 401
 402 void p2p_buf_add_session_info(struct wpabuf *buf, const char *info)
 403 {
 404         size_t info_len = 0;
 405
 406         if (info && info[0])
 407                 info_len = os_strlen(info);
 408
 409         /* Session Information Data Info */
 410         wpabuf_put_u8(buf, P2P_ATTR_SESSION_INFORMATION_DATA);
 411         wpabuf_put_le16(buf, (u16) info_len);
 412
 413         if (info) {
 414                 wpabuf_put_data(buf, info, info_len);
 415                 wpa_printf(MSG_DEBUG, "P2P: * Session Info Data (%s)", info);
 416         }
 417 }
 418
 419
 420 void p2p_buf_add_connection_capability(struct wpabuf *buf, u8 connection_cap)
 421 {
 422         /* Connection Capability Info */
 423         wpabuf_put_u8(buf, P2P_ATTR_CONNECTION_CAPABILITY);
 424         wpabuf_put_le16(buf, 1);
 425         wpabuf_put_u8(buf, connection_cap);
 426         wpa_printf(MSG_DEBUG, "P2P: * Connection Capability: 0x%x",
 427                    connection_cap);
 428 }
 429
 430
 431 void p2p_buf_add_advertisement_id(struct wpabuf *buf, u32 id, const u8 *mac)
 432 {
 433         if (!buf || !mac)
 434                 return;
 435
 436         /* Advertisement ID Info */
 437         wpabuf_put_u8(buf, P2P_ATTR_ADVERTISEMENT_ID);
 438         wpabuf_put_le16(buf, (u16) (sizeof(u32) + ETH_ALEN));
 439         wpabuf_put_le32(buf, id);
 440         wpabuf_put_data(buf, mac, ETH_ALEN);
 441         wpa_printf(MSG_DEBUG, "P2P: * Advertisement ID (%x) " MACSTR,
 442                    id, MAC2STR(mac));
 443 }
 444
 445
 446 static int p2ps_wildcard_hash(struct p2p_data *p2p,
 447                               const u8 *hash, u8 hash_count)
 448 {
 449         u8 i;
 450         const u8 *test = hash;
 451
 452         for (i = 0; i < hash_count; i++) {
 453                 if (os_memcmp(test, p2p->wild_card_hash, P2PS_HASH_LEN) == 0)
 454                         return 1;
 455                 test += P2PS_HASH_LEN;
 456         }
 457
 458         return 0;
 459 }
 460
 461
 462 static int p2p_wfa_service_adv(struct p2p_data *p2p)
 463 {
 464         struct p2ps_advertisement *adv;
 465
 466         for (adv = p2p->p2ps_adv_list; adv; adv = adv->next) {
 467                 if (os_strncmp(adv->svc_name, P2PS_WILD_HASH_STR,
 468                                os_strlen(P2PS_WILD_HASH_STR)) == 0)
 469                         return 1;
 470         }
 471
 472         return 0;
 473 }
 474
 475
 476 static int p2p_buf_add_service_info(struct wpabuf *buf, struct p2p_data *p2p,
 477                                     u32 adv_id, u16 config_methods,
 478                                     const char *svc_name, u8 **ie_len, u8 **pos,
 479                                     size_t *total_len, u8 *attr_len)
 480 {
 481         size_t svc_len;
 482         size_t remaining;
 483         size_t info_len;
 484
 485         p2p_dbg(p2p, "Add service info for %s (adv_id=%u)", svc_name, adv_id);
 486         svc_len = os_strlen(svc_name);
 487         info_len = sizeof(adv_id) + sizeof(config_methods) + sizeof(u8) +
 488                 svc_len;
 489
 490         if (info_len + *total_len > MAX_SVC_ADV_LEN) {
 491                 p2p_dbg(p2p,
 492                         "Unsufficient buffer, failed to add advertised service info");
 493                 return -1;
 494         }
 495
 496         if (svc_len > 255) {
 497                 p2p_dbg(p2p,
 498                         "Invalid service name length (%u bytes), failed to add advertised service info",
 499                         (unsigned int) svc_len);
 500                 return -1;
 501         }
 502
 503         if (*ie_len) {
 504                 int ie_data_len = (*pos - *ie_len) - 1;
 505
 506                 if (ie_data_len < 0 || ie_data_len > 255) {
 507                         p2p_dbg(p2p,
 508                                 "Invalid IE length, failed to add advertised service info");
 509                         return -1;
 510                 }
 511                 remaining = 255 - ie_data_len;
 512         } else {
 513                 /*
 514                  * Adding new P2P IE header takes 6 extra bytes:
 515                  * - 2 byte IE header (1 byte IE id and 1 byte length)
 516                  * - 4 bytes of IE_VENDOR_TYPE are reduced from 255 below
 517                  */
 518                 *ie_len = p2p_buf_add_ie_hdr(buf);
 519                 remaining = 255 - 4;
 520         }
 521
 522         if (remaining < sizeof(u32) + sizeof(u16) + sizeof(u8)) {
 523                 /*
 524                  * Split adv_id, config_methods, and svc_name_len between two
 525                  * IEs.
 526                  */
 527                 size_t front = remaining;
 528                 size_t back = sizeof(u32) + sizeof(u16) + sizeof(u8) - front;
 529                 u8 holder[sizeof(u32) + sizeof(u16) + sizeof(u8)];
 530
 531                 WPA_PUT_LE32(holder, adv_id);
 532                 WPA_PUT_BE16(&holder[sizeof(u32)], config_methods);
 533                 holder[sizeof(u32) + sizeof(u16)] = svc_len;
 534
 535                 if (front)
 536                         wpabuf_put_data(buf, holder, front);
 537
 538                 p2p_buf_update_ie_hdr(buf, *ie_len);
 539                 *ie_len = p2p_buf_add_ie_hdr(buf);
 540
 541                 wpabuf_put_data(buf, &holder[front], back);
 542                 remaining = 255 - 4 - (sizeof(u32) + sizeof(u16) + sizeof(u8)) -
 543                         back;
 544         } else {
 545                 wpabuf_put_le32(buf, adv_id);
 546                 wpabuf_put_be16(buf, config_methods);
 547                 wpabuf_put_u8(buf, svc_len);
 548                 remaining -= sizeof(adv_id) + sizeof(config_methods) +
 549                         sizeof(u8);
 550         }
 551
 552         if (remaining < svc_len) {
 553                 /* split svc_name between two or three IEs */
 554                 size_t front = remaining;
 555                 size_t back = svc_len - front;
 556
 557                 if (front)
 558                         wpabuf_put_data(buf, svc_name, front);
 559
 560                 p2p_buf_update_ie_hdr(buf, *ie_len);
 561                 *ie_len = p2p_buf_add_ie_hdr(buf);
 562
 563                 /* In rare cases, we must split across 3 attributes */
 564                 if (back > 255 - 4) {
 565                         wpabuf_put_data(buf, &svc_name[front], 255 - 4);
 566                         back -= 255 - 4;
 567                         front += 255 - 4;
 568                         p2p_buf_update_ie_hdr(buf, *ie_len);
 569                         *ie_len = p2p_buf_add_ie_hdr(buf);
 570                 }
 571
 572                 wpabuf_put_data(buf, &svc_name[front], back);
 573                 remaining = 255 - 4 - back;
 574         } else {
 575                 wpabuf_put_data(buf, svc_name, svc_len);
 576                 remaining -= svc_len;
 577         }
 578
 579         p2p_buf_update_ie_hdr(buf, *ie_len);
 580
 581         /* set *ie_len to NULL if a new IE has to be added on the next call */
 582         if (!remaining)
 583                 *ie_len = NULL;
 584
 585         /* set *pos to point to the next byte to update */
 586         *pos = wpabuf_put(buf, 0);
 587
 588         *total_len += info_len;
 589         WPA_PUT_LE16(attr_len, (u16) *total_len);
 590         return 0;
 591 }
 592
 593
 594 void p2p_buf_add_service_instance(struct wpabuf *buf, struct p2p_data *p2p,
 595                                   u8 hash_count, const u8 *hash,
 596                                   struct p2ps_advertisement *adv_list)
 597 {
 598         struct p2ps_advertisement *adv;
 599         int p2ps_wildcard;
 600         size_t total_len;
 601         struct wpabuf *tmp_buf = NULL;
 602         u8 *pos, *attr_len, *ie_len = NULL;
 603
 604         if (!adv_list || !hash || !hash_count)
 605                 return;
 606
 607         wpa_hexdump(MSG_DEBUG, "P2PS: Probe Request service hash values",
 608                     hash, hash_count * P2PS_HASH_LEN);
 609         p2ps_wildcard = p2ps_wildcard_hash(p2p, hash, hash_count) &&
 610                 p2p_wfa_service_adv(p2p);
 611
 612         /* Allocate temp buffer, allowing for overflow of 1 instance */
 613         tmp_buf = wpabuf_alloc(MAX_SVC_ADV_IE_LEN + 256 + P2PS_HASH_LEN);
 614         if (!tmp_buf)
 615                 return;
 616
 617         /*
 618          * Attribute data can be split into a number of IEs. Start with the
 619          * first IE and the attribute headers here.
 620          */
 621         ie_len = p2p_buf_add_ie_hdr(tmp_buf);
 622
 623         total_len = 0;
 624
 625         wpabuf_put_u8(tmp_buf, P2P_ATTR_ADVERTISED_SERVICE);
 626         attr_len = wpabuf_put(tmp_buf, sizeof(u16));
 627         WPA_PUT_LE16(attr_len, (u16) total_len);
 628         p2p_buf_update_ie_hdr(tmp_buf, ie_len);
 629         pos = wpabuf_put(tmp_buf, 0);
 630
 631         if (p2ps_wildcard) {
 632                 /* org.wi-fi.wfds match found */
 633                 p2p_buf_add_service_info(tmp_buf, p2p, 0, 0, P2PS_WILD_HASH_STR,
 634                                          &ie_len, &pos, &total_len, attr_len);
 635         }
 636
 637         /* add advertised service info of matching services */
 638         for (adv = adv_list; adv && total_len <= MAX_SVC_ADV_LEN;
 639              adv = adv->next) {
 640                 const u8 *test = hash;
 641                 u8 i;
 642
 643                 for (i = 0; i < hash_count; i++) {
 644                         /* exact name hash match */
 645                         if (os_memcmp(test, adv->hash, P2PS_HASH_LEN) == 0 &&
 646                             p2p_buf_add_service_info(tmp_buf, p2p,
 647                                                      adv->id,
 648                                                      adv->config_methods,
 649                                                      adv->svc_name,
 650                                                      &ie_len, &pos,
 651                                                      &total_len,
 652                                                      attr_len))
 653                                 break;
 654
 655                         test += P2PS_HASH_LEN;
 656                 }
 657         }
 658
 659         if (total_len)
 660                 wpabuf_put_buf(buf, tmp_buf);
 661         wpabuf_free(tmp_buf);
 662 }
 663
 664
 665 void p2p_buf_add_session_id(struct wpabuf *buf, u32 id, const u8 *mac)
 666 {
 667         if (!buf || !mac)
 668                 return;
 669
 670         /* Session ID Info */
 671         wpabuf_put_u8(buf, P2P_ATTR_SESSION_ID);
 672         wpabuf_put_le16(buf, (u16) (sizeof(u32) + ETH_ALEN));
 673         wpabuf_put_le32(buf, id);
 674         wpabuf_put_data(buf, mac, ETH_ALEN);
 675         wpa_printf(MSG_DEBUG, "P2P: * Session ID Info (%x) " MACSTR,
 676                    id, MAC2STR(mac));
 677 }
 678
 679
 680 void p2p_buf_add_feature_capability(struct wpabuf *buf, u16 len, const u8 *mask)
 681 {
 682         if (!buf || !len || !mask)
 683                 return;
 684
 685         /* Feature Capability */
 686         wpabuf_put_u8(buf, P2P_ATTR_FEATURE_CAPABILITY);
 687         wpabuf_put_le16(buf, len);
 688         wpabuf_put_data(buf, mask, len);
 689         wpa_printf(MSG_DEBUG, "P2P: * Feature Capability (%d)", len);
 690 }
 691
 692
 693 void p2p_buf_add_persistent_group_info(struct wpabuf *buf, const u8 *dev_addr,
 694                                        const u8 *ssid, size_t ssid_len)
 695 {
 696         /* P2P Group ID */
 697         wpabuf_put_u8(buf, P2P_ATTR_PERSISTENT_GROUP);
 698         wpabuf_put_le16(buf, ETH_ALEN + ssid_len);
 699         wpabuf_put_data(buf, dev_addr, ETH_ALEN);
 700         wpabuf_put_data(buf, ssid, ssid_len);
 701         wpa_printf(MSG_DEBUG, "P2P: * P2P Group ID " MACSTR,
 702                    MAC2STR(dev_addr));
 703 }
 704
 705
 706 static int p2p_add_wps_string(struct wpabuf *buf, enum wps_attribute attr,
 707                               const char *val)
 708 {
 709         size_t len;
 710
 711         len = val ? os_strlen(val) : 0;
 712         if (wpabuf_tailroom(buf) < 4 + len)
 713                 return -1;
 714         wpabuf_put_be16(buf, attr);
 715 #ifndef CONFIG_WPS_STRICT
 716         if (len == 0) {
 717                 /*
 718                  * Some deployed WPS implementations fail to parse zeor-length
 719                  * attributes. As a workaround, send a space character if the
 720                  * device attribute string is empty.
 721                  */
 722                 if (wpabuf_tailroom(buf) < 3)
 723                         return -1;
 724                 wpabuf_put_be16(buf, 1);
 725                 wpabuf_put_u8(buf, ' ');
 726                 return 0;
 727         }
 728 #endif /* CONFIG_WPS_STRICT */
 729         wpabuf_put_be16(buf, len);
 730         if (val)
 731                 wpabuf_put_data(buf, val, len);
 732         return 0;
 733 }
 734
 735
 736 int p2p_build_wps_ie(struct p2p_data *p2p, struct wpabuf *buf, int pw_id,
 737                      int all_attr)
 738 {
 739         u8 *len;
 740         int i;
 741
 742         if (wpabuf_tailroom(buf) < 6)
 743                 return -1;
 744         wpabuf_put_u8(buf, WLAN_EID_VENDOR_SPECIFIC);
 745         len = wpabuf_put(buf, 1);
 746         wpabuf_put_be32(buf, WPS_DEV_OUI_WFA);
 747
 748         if (wps_build_version(buf) < 0)
 749                 return -1;
 750
 751         if (all_attr) {
 752                 if (wpabuf_tailroom(buf) < 5)
 753                         return -1;
 754                 wpabuf_put_be16(buf, ATTR_WPS_STATE);
 755                 wpabuf_put_be16(buf, 1);
 756                 wpabuf_put_u8(buf, WPS_STATE_NOT_CONFIGURED);
 757         }
 758
 759         if (pw_id >= 0) {
 760                 if (wpabuf_tailroom(buf) < 6)
 761                         return -1;
 762                 /* Device Password ID */
 763                 wpabuf_put_be16(buf, ATTR_DEV_PASSWORD_ID);
 764                 wpabuf_put_be16(buf, 2);
 765                 wpa_printf(MSG_DEBUG, "P2P: WPS IE Device Password ID: %d",
 766                            pw_id);
 767                 wpabuf_put_be16(buf, pw_id);
 768         }
 769
 770         if (all_attr) {
 771                 if (wpabuf_tailroom(buf) < 5)
 772                         return -1;
 773                 wpabuf_put_be16(buf, ATTR_RESPONSE_TYPE);
 774                 wpabuf_put_be16(buf, 1);
 775                 wpabuf_put_u8(buf, WPS_RESP_ENROLLEE_INFO);
 776
 777                 if (wps_build_uuid_e(buf, p2p->cfg->uuid) < 0 ||
 778                     p2p_add_wps_string(buf, ATTR_MANUFACTURER,
 779                                        p2p->cfg->manufacturer) < 0 ||
 780                     p2p_add_wps_string(buf, ATTR_MODEL_NAME,
 781                                        p2p->cfg->model_name) < 0 ||
 782                     p2p_add_wps_string(buf, ATTR_MODEL_NUMBER,
 783                                        p2p->cfg->model_number) < 0 ||
 784                     p2p_add_wps_string(buf, ATTR_SERIAL_NUMBER,
 785                                        p2p->cfg->serial_number) < 0)
 786                         return -1;
 787
 788                 if (wpabuf_tailroom(buf) < 4 + WPS_DEV_TYPE_LEN)
 789                         return -1;
 790                 wpabuf_put_be16(buf, ATTR_PRIMARY_DEV_TYPE);
 791                 wpabuf_put_be16(buf, WPS_DEV_TYPE_LEN);
 792                 wpabuf_put_data(buf, p2p->cfg->pri_dev_type, WPS_DEV_TYPE_LEN);
 793
 794                 if (p2p_add_wps_string(buf, ATTR_DEV_NAME, p2p->cfg->dev_name)
 795                     < 0)
 796                         return -1;
 797
 798                 if (wpabuf_tailroom(buf) < 6)
 799                         return -1;
 800                 wpabuf_put_be16(buf, ATTR_CONFIG_METHODS);
 801                 wpabuf_put_be16(buf, 2);
 802                 wpabuf_put_be16(buf, p2p->cfg->config_methods);
 803         }
 804
 805         if (wps_build_wfa_ext(buf, 0, NULL, 0) < 0)
 806                 return -1;
 807
 808         if (all_attr && p2p->cfg->num_sec_dev_types) {
 809                 if (wpabuf_tailroom(buf) <
 810                     4 + WPS_DEV_TYPE_LEN * p2p->cfg->num_sec_dev_types)
 811                         return -1;
 812                 wpabuf_put_be16(buf, ATTR_SECONDARY_DEV_TYPE_LIST);
 813                 wpabuf_put_be16(buf, WPS_DEV_TYPE_LEN *
 814                                 p2p->cfg->num_sec_dev_types);
 815                 wpabuf_put_data(buf, p2p->cfg->sec_dev_type,
 816                                 WPS_DEV_TYPE_LEN *
 817                                 p2p->cfg->num_sec_dev_types);
 818         }
 819
 820         /* Add the WPS vendor extensions */
 821         for (i = 0; i < P2P_MAX_WPS_VENDOR_EXT; i++) {
 822                 if (p2p->wps_vendor_ext[i] == NULL)
 823                         break;
 824                 if (wpabuf_tailroom(buf) <
 825                     4 + wpabuf_len(p2p->wps_vendor_ext[i]))
 826                         continue;
 827                 wpabuf_put_be16(buf, ATTR_VENDOR_EXT);
 828                 wpabuf_put_be16(buf, wpabuf_len(p2p->wps_vendor_ext[i]));
 829                 wpabuf_put_buf(buf, p2p->wps_vendor_ext[i]);
 830         }
 831
 832         p2p_buf_update_ie_hdr(buf, len);
 833
 834         return 0;
 835 }