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