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