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