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
17void 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
28void 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
41u8 * 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
54void 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
61void 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
73void 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
84void 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
98void 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
112void 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
137void 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
147void 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
206void 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
216void 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
229void 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
240void 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
251void 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
265void 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
275static 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
287void 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
301void 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
314void 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
331void 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
348void 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
363void 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
381void 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
392void 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
407static 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
423static 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
437static 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
555void 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
626void 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
641void 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
654void 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
667static 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
697int 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