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; 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 } 597 598 /* add advertised service info of matching services */ 599 for (adv = adv_list; adv && total_len <= MAX_SVC_ADV_LEN; 600 adv = adv->next) { 601 const u8 *test = hash; 602 u8 i; 603 604 for (i = 0; i < hash_count; i++) { 605 /* exact name hash match */ 606 if (os_memcmp(test, adv->hash, P2PS_HASH_LEN) == 0 && 607 p2p_buf_add_service_info(tmp_buf, p2p, 608 adv->id, 609 adv->config_methods, 610 adv->svc_name, 611 &ie_len, &pos, 612 &total_len, 613 attr_len)) 614 break; 615 616 test += P2PS_HASH_LEN; 617 } 618 } 619 620 if (total_len) 621 wpabuf_put_buf(buf, tmp_buf); 622 wpabuf_free(tmp_buf); 623 } 624 625 626 void p2p_buf_add_session_id(struct wpabuf *buf, u32 id, const u8 *mac) 627 { 628 if (!buf || !mac) 629 return; 630 631 /* Session ID Info */ 632 wpabuf_put_u8(buf, P2P_ATTR_SESSION_ID); 633 wpabuf_put_le16(buf, (u16) (sizeof(u32) + ETH_ALEN)); 634 wpabuf_put_le32(buf, id); 635 wpabuf_put_data(buf, mac, ETH_ALEN); 636 wpa_printf(MSG_DEBUG, "P2P: * Session ID Info (%x) " MACSTR, 637 id, MAC2STR(mac)); 638 } 639 640 641 void p2p_buf_add_feature_capability(struct wpabuf *buf, u16 len, const u8 *mask) 642 { 643 if (!buf || !len || !mask) 644 return; 645 646 /* Feature Capability */ 647 wpabuf_put_u8(buf, P2P_ATTR_FEATURE_CAPABILITY); 648 wpabuf_put_le16(buf, len); 649 wpabuf_put_data(buf, mask, len); 650 wpa_printf(MSG_DEBUG, "P2P: * Feature Capability (%d)", len); 651 } 652 653 654 void p2p_buf_add_persistent_group_info(struct wpabuf *buf, const u8 *dev_addr, 655 const u8 *ssid, size_t ssid_len) 656 { 657 /* P2P Group ID */ 658 wpabuf_put_u8(buf, P2P_ATTR_PERSISTENT_GROUP); 659 wpabuf_put_le16(buf, ETH_ALEN + ssid_len); 660 wpabuf_put_data(buf, dev_addr, ETH_ALEN); 661 wpabuf_put_data(buf, ssid, ssid_len); 662 wpa_printf(MSG_DEBUG, "P2P: * P2P Group ID " MACSTR, 663 MAC2STR(dev_addr)); 664 } 665 666 667 static int p2p_add_wps_string(struct wpabuf *buf, enum wps_attribute attr, 668 const char *val) 669 { 670 size_t len; 671 672 len = val ? os_strlen(val) : 0; 673 if (wpabuf_tailroom(buf) < 4 + len) 674 return -1; 675 wpabuf_put_be16(buf, attr); 676 #ifndef CONFIG_WPS_STRICT 677 if (len == 0) { 678 /* 679 * Some deployed WPS implementations fail to parse zeor-length 680 * attributes. As a workaround, send a space character if the 681 * device attribute string is empty. 682 */ 683 if (wpabuf_tailroom(buf) < 3) 684 return -1; 685 wpabuf_put_be16(buf, 1); 686 wpabuf_put_u8(buf, ' '); 687 return 0; 688 } 689 #endif /* CONFIG_WPS_STRICT */ 690 wpabuf_put_be16(buf, len); 691 if (val) 692 wpabuf_put_data(buf, val, len); 693 return 0; 694 } 695 696 697 int p2p_build_wps_ie(struct p2p_data *p2p, struct wpabuf *buf, int pw_id, 698 int all_attr) 699 { 700 u8 *len; 701 int i; 702 703 if (wpabuf_tailroom(buf) < 6) 704 return -1; 705 wpabuf_put_u8(buf, WLAN_EID_VENDOR_SPECIFIC); 706 len = wpabuf_put(buf, 1); 707 wpabuf_put_be32(buf, WPS_DEV_OUI_WFA); 708 709 if (wps_build_version(buf) < 0) 710 return -1; 711 712 if (all_attr) { 713 if (wpabuf_tailroom(buf) < 5) 714 return -1; 715 wpabuf_put_be16(buf, ATTR_WPS_STATE); 716 wpabuf_put_be16(buf, 1); 717 wpabuf_put_u8(buf, WPS_STATE_NOT_CONFIGURED); 718 } 719 720 if (pw_id >= 0) { 721 if (wpabuf_tailroom(buf) < 6) 722 return -1; 723 /* Device Password ID */ 724 wpabuf_put_be16(buf, ATTR_DEV_PASSWORD_ID); 725 wpabuf_put_be16(buf, 2); 726 wpa_printf(MSG_DEBUG, "P2P: WPS IE Device Password ID: %d", 727 pw_id); 728 wpabuf_put_be16(buf, pw_id); 729 } 730 731 if (all_attr) { 732 if (wpabuf_tailroom(buf) < 5) 733 return -1; 734 wpabuf_put_be16(buf, ATTR_RESPONSE_TYPE); 735 wpabuf_put_be16(buf, 1); 736 wpabuf_put_u8(buf, WPS_RESP_ENROLLEE_INFO); 737 738 if (wps_build_uuid_e(buf, p2p->cfg->uuid) < 0 || 739 p2p_add_wps_string(buf, ATTR_MANUFACTURER, 740 p2p->cfg->manufacturer) < 0 || 741 p2p_add_wps_string(buf, ATTR_MODEL_NAME, 742 p2p->cfg->model_name) < 0 || 743 p2p_add_wps_string(buf, ATTR_MODEL_NUMBER, 744 p2p->cfg->model_number) < 0 || 745 p2p_add_wps_string(buf, ATTR_SERIAL_NUMBER, 746 p2p->cfg->serial_number) < 0) 747 return -1; 748 749 if (wpabuf_tailroom(buf) < 4 + WPS_DEV_TYPE_LEN) 750 return -1; 751 wpabuf_put_be16(buf, ATTR_PRIMARY_DEV_TYPE); 752 wpabuf_put_be16(buf, WPS_DEV_TYPE_LEN); 753 wpabuf_put_data(buf, p2p->cfg->pri_dev_type, WPS_DEV_TYPE_LEN); 754 755 if (p2p_add_wps_string(buf, ATTR_DEV_NAME, p2p->cfg->dev_name) 756 < 0) 757 return -1; 758 759 if (wpabuf_tailroom(buf) < 6) 760 return -1; 761 wpabuf_put_be16(buf, ATTR_CONFIG_METHODS); 762 wpabuf_put_be16(buf, 2); 763 wpabuf_put_be16(buf, p2p->cfg->config_methods); 764 } 765 766 if (wps_build_wfa_ext(buf, 0, NULL, 0) < 0) 767 return -1; 768 769 if (all_attr && p2p->cfg->num_sec_dev_types) { 770 if (wpabuf_tailroom(buf) < 771 4 + WPS_DEV_TYPE_LEN * p2p->cfg->num_sec_dev_types) 772 return -1; 773 wpabuf_put_be16(buf, ATTR_SECONDARY_DEV_TYPE_LIST); 774 wpabuf_put_be16(buf, WPS_DEV_TYPE_LEN * 775 p2p->cfg->num_sec_dev_types); 776 wpabuf_put_data(buf, p2p->cfg->sec_dev_type, 777 WPS_DEV_TYPE_LEN * 778 p2p->cfg->num_sec_dev_types); 779 } 780 781 /* Add the WPS vendor extensions */ 782 for (i = 0; i < P2P_MAX_WPS_VENDOR_EXT; i++) { 783 if (p2p->wps_vendor_ext[i] == NULL) 784 break; 785 if (wpabuf_tailroom(buf) < 786 4 + wpabuf_len(p2p->wps_vendor_ext[i])) 787 continue; 788 wpabuf_put_be16(buf, ATTR_VENDOR_EXT); 789 wpabuf_put_be16(buf, wpabuf_len(p2p->wps_vendor_ext[i])); 790 wpabuf_put_buf(buf, p2p->wps_vendor_ext[i]); 791 } 792 793 p2p_buf_update_ie_hdr(buf, len); 794 795 return 0; 796 } 797
