p2p_build.c 22 KB

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