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