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 unsigned int *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_P2PS) 206 methods |= WPS_CONFIG_P2PS; 207 else if (peer->wps_method == WPS_PIN_DISPLAY || 208 peer->wps_method == WPS_PIN_KEYPAD) 209 methods |= WPS_CONFIG_DISPLAY | WPS_CONFIG_KEYPAD; 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) < 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 } 836