• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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 
p2p_buf_add_action_hdr(struct wpabuf * buf,u8 subtype,u8 dialog_token)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 
p2p_buf_add_public_action_hdr(struct wpabuf * buf,u8 subtype,u8 dialog_token)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 
p2p_buf_add_ie_hdr(struct wpabuf * buf)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 
p2p_buf_update_ie_hdr(struct wpabuf * buf,u8 * len)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 
p2p_buf_add_capability(struct wpabuf * buf,u8 dev_capab,u8 group_capab)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 
p2p_buf_add_go_intent(struct wpabuf * buf,u8 go_intent)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 
p2p_buf_add_listen_channel(struct wpabuf * buf,const char * country,u8 reg_class,u8 channel)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 
p2p_buf_add_operating_channel(struct wpabuf * buf,const char * country,u8 reg_class,u8 channel)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 
p2p_buf_add_pref_channel_list(struct wpabuf * buf,const u32 * preferred_freq_list,unsigned int size)113 void p2p_buf_add_pref_channel_list(struct wpabuf *buf,
114 				   const u32 *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 
p2p_buf_add_channel_list(struct wpabuf * buf,const char * country,struct p2p_channels * chan)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 
p2p_buf_add_status(struct wpabuf * buf,u8 status)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 
p2p_buf_add_device_info(struct wpabuf * buf,struct p2p_data * p2p,struct p2p_device * peer)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 
p2p_buf_add_device_id(struct wpabuf * buf,const u8 * dev_addr)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 
p2p_buf_add_config_timeout(struct wpabuf * buf,u8 go_timeout,u8 client_timeout)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 
p2p_buf_add_intended_addr(struct wpabuf * buf,const u8 * interface_addr)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 
p2p_buf_add_group_bssid(struct wpabuf * buf,const u8 * bssid)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 
p2p_buf_add_group_id(struct wpabuf * buf,const u8 * dev_addr,const u8 * ssid,size_t ssid_len)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 
p2p_buf_add_invitation_flags(struct wpabuf * buf,u8 flags)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 
p2p_buf_add_noa_desc(struct wpabuf * buf,struct p2p_noa_desc * desc)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 
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)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 
p2p_buf_add_ext_listen_timing(struct wpabuf * buf,u16 period,u16 interval)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 
p2p_buf_add_p2p_interface(struct wpabuf * buf,struct p2p_data * p2p)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 
p2p_buf_add_oob_go_neg_channel(struct wpabuf * buf,const char * country,u8 oper_class,u8 channel,enum p2p_role_indication role)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 
p2p_buf_add_service_hash(struct wpabuf * buf,struct p2p_data * p2p)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 
p2p_buf_add_session_info(struct wpabuf * buf,const char * info)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 
p2p_buf_add_connection_capability(struct wpabuf * buf,u8 connection_cap)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 
p2p_buf_add_advertisement_id(struct wpabuf * buf,u32 id,const u8 * mac)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 
p2ps_wildcard_hash(struct p2p_data * p2p,const u8 * hash,u8 hash_count)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 
p2p_wfa_service_adv(struct p2p_data * p2p)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 
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)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 
p2p_buf_add_service_instance(struct wpabuf * buf,struct p2p_data * p2p,u8 hash_count,const u8 * hash,struct p2ps_advertisement * adv_list)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 
p2p_buf_add_session_id(struct wpabuf * buf,u32 id,const u8 * mac)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 
p2p_buf_add_feature_capability(struct wpabuf * buf,u16 len,const u8 * mask)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 
p2p_buf_add_persistent_group_info(struct wpabuf * buf,const u8 * dev_addr,const u8 * ssid,size_t ssid_len)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 
p2p_add_wps_string(struct wpabuf * buf,enum wps_attribute attr,const char * val)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 
p2p_build_wps_ie(struct p2p_data * p2p,struct wpabuf * buf,int pw_id,int all_attr)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)
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