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
p2p_buf_add_action_hdr(struct wpabuf * buf,u8 subtype,u8 dialog_token)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
p2p_buf_add_public_action_hdr(struct wpabuf * buf,u8 subtype,u8 dialog_token)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
p2p_buf_add_ie_hdr(struct wpabuf * buf)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
p2p_buf_update_ie_hdr(struct wpabuf * buf,u8 * len)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
p2p_buf_add_capability(struct wpabuf * buf,u8 dev_capab,u8 group_capab)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
p2p_buf_add_go_intent(struct wpabuf * buf,u8 go_intent)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
p2p_buf_add_listen_channel(struct wpabuf * buf,const char * country,u8 reg_class,u8 channel)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
p2p_buf_add_operating_channel(struct wpabuf * buf,const char * country,u8 reg_class,u8 channel)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
p2p_buf_add_channel_list(struct wpabuf * buf,const char * country,struct p2p_channels * chan)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
p2p_buf_add_status(struct wpabuf * buf,u8 status)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
p2p_buf_add_device_info(struct wpabuf * buf,struct p2p_data * p2p,struct p2p_device * peer)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
p2p_buf_add_device_id(struct wpabuf * buf,const u8 * dev_addr)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
p2p_buf_add_config_timeout(struct wpabuf * buf,u8 go_timeout,u8 client_timeout)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
p2p_buf_add_intended_addr(struct wpabuf * buf,const u8 * interface_addr)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
p2p_buf_add_group_bssid(struct wpabuf * buf,const u8 * bssid)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
p2p_buf_add_group_id(struct wpabuf * buf,const u8 * dev_addr,const u8 * ssid,size_t ssid_len)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
p2p_buf_add_invitation_flags(struct wpabuf * buf,u8 flags)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
p2p_buf_add_noa_desc(struct wpabuf * buf,struct p2p_noa_desc * desc)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
p2p_buf_add_noa(struct wpabuf * buf,u8 noa_index,u8 opp_ps,u8 ctwindow,struct p2p_noa_desc * desc1,struct p2p_noa_desc * desc2)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
p2p_buf_add_ext_listen_timing(struct wpabuf * buf,u16 period,u16 interval)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
p2p_buf_add_p2p_interface(struct wpabuf * buf,struct p2p_data * p2p)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
p2p_buf_add_oob_go_neg_channel(struct wpabuf * buf,const char * country,u8 oper_class,u8 channel,enum p2p_role_indication role)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
p2p_buf_add_service_hash(struct wpabuf * buf,struct p2p_data * p2p)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
p2p_buf_add_session_info(struct wpabuf * buf,const char * info)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
p2p_buf_add_connection_capability(struct wpabuf * buf,u8 connection_cap)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
p2p_buf_add_advertisement_id(struct wpabuf * buf,u32 id,const u8 * mac)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
p2ps_wildcard_hash(struct p2p_data * p2p,const u8 * hash,u8 hash_count)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
p2p_wfa_service_adv(struct p2p_data * p2p)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
p2p_buf_add_service_info(struct wpabuf * buf,struct p2p_data * p2p,u32 adv_id,u16 config_methods,const char * svc_name,u8 ** ie_len,u8 ** pos,size_t * total_len,u8 * attr_len)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
p2p_buf_add_service_instance(struct wpabuf * buf,struct p2p_data * p2p,u8 hash_count,const u8 * hash,struct p2ps_advertisement * adv_list)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
p2p_buf_add_session_id(struct wpabuf * buf,u32 id,const u8 * mac)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
p2p_buf_add_feature_capability(struct wpabuf * buf,u16 len,const u8 * mask)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
p2p_buf_add_persistent_group_info(struct wpabuf * buf,const u8 * dev_addr,const u8 * ssid,size_t ssid_len)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
p2p_add_wps_string(struct wpabuf * buf,enum wps_attribute attr,const char * val)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
p2p_build_wps_ie(struct p2p_data * p2p,struct wpabuf * buf,int pw_id,int all_attr)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