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) < 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