ctrl.c 34 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358
  1. /*
  2. * wlantest control interface
  3. * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
  4. *
  5. * This program is free software; you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License version 2 as
  7. * published by the Free Software Foundation.
  8. *
  9. * Alternatively, this software may be distributed under the terms of BSD
  10. * license.
  11. *
  12. * See README and COPYING for more details.
  13. */
  14. #include "utils/includes.h"
  15. #include <sys/un.h>
  16. #include "utils/common.h"
  17. #include "utils/eloop.h"
  18. #include "common/defs.h"
  19. #include "common/version.h"
  20. #include "common/ieee802_11_defs.h"
  21. #include "wlantest.h"
  22. #include "wlantest_ctrl.h"
  23. static u8 * attr_get(u8 *buf, size_t buflen, enum wlantest_ctrl_attr attr,
  24. size_t *len)
  25. {
  26. u8 *pos = buf;
  27. while (pos + 8 <= buf + buflen) {
  28. enum wlantest_ctrl_attr a;
  29. size_t alen;
  30. a = WPA_GET_BE32(pos);
  31. pos += 4;
  32. alen = WPA_GET_BE32(pos);
  33. pos += 4;
  34. if (pos + alen > buf + buflen) {
  35. wpa_printf(MSG_DEBUG, "Invalid control message "
  36. "attribute");
  37. return NULL;
  38. }
  39. if (a == attr) {
  40. *len = alen;
  41. return pos;
  42. }
  43. pos += alen;
  44. }
  45. return NULL;
  46. }
  47. static u8 * attr_get_macaddr(u8 *buf, size_t buflen,
  48. enum wlantest_ctrl_attr attr)
  49. {
  50. u8 *addr;
  51. size_t addr_len;
  52. addr = attr_get(buf, buflen, attr, &addr_len);
  53. if (addr && addr_len != ETH_ALEN)
  54. addr = NULL;
  55. return addr;
  56. }
  57. static int attr_get_int(u8 *buf, size_t buflen, enum wlantest_ctrl_attr attr)
  58. {
  59. u8 *pos;
  60. size_t len;
  61. pos = attr_get(buf, buflen, attr, &len);
  62. if (pos == NULL || len != 4)
  63. return -1;
  64. return WPA_GET_BE32(pos);
  65. }
  66. static u8 * attr_add_str(u8 *pos, u8 *end, enum wlantest_ctrl_attr attr,
  67. const char *str)
  68. {
  69. size_t len = os_strlen(str);
  70. if (pos == NULL || end - pos < 8 + len)
  71. return NULL;
  72. WPA_PUT_BE32(pos, attr);
  73. pos += 4;
  74. WPA_PUT_BE32(pos, len);
  75. pos += 4;
  76. os_memcpy(pos, str, len);
  77. pos += len;
  78. return pos;
  79. }
  80. static u8 * attr_add_be32(u8 *pos, u8 *end, enum wlantest_ctrl_attr attr,
  81. u32 val)
  82. {
  83. if (pos == NULL || end - pos < 12)
  84. return NULL;
  85. WPA_PUT_BE32(pos, attr);
  86. pos += 4;
  87. WPA_PUT_BE32(pos, 4);
  88. pos += 4;
  89. WPA_PUT_BE32(pos, val);
  90. pos += 4;
  91. return pos;
  92. }
  93. static void ctrl_disconnect(struct wlantest *wt, int sock)
  94. {
  95. int i;
  96. wpa_printf(MSG_DEBUG, "Disconnect control interface connection %d",
  97. sock);
  98. for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
  99. if (wt->ctrl_socks[i] == sock) {
  100. close(wt->ctrl_socks[i]);
  101. eloop_unregister_read_sock(wt->ctrl_socks[i]);
  102. wt->ctrl_socks[i] = -1;
  103. break;
  104. }
  105. }
  106. }
  107. static void ctrl_send(struct wlantest *wt, int sock, const u8 *buf,
  108. size_t len)
  109. {
  110. if (send(sock, buf, len, 0) < 0) {
  111. wpa_printf(MSG_INFO, "send(ctrl): %s", strerror(errno));
  112. ctrl_disconnect(wt, sock);
  113. }
  114. }
  115. static void ctrl_send_simple(struct wlantest *wt, int sock,
  116. enum wlantest_ctrl_cmd cmd)
  117. {
  118. u8 buf[4];
  119. WPA_PUT_BE32(buf, cmd);
  120. ctrl_send(wt, sock, buf, sizeof(buf));
  121. }
  122. static struct wlantest_bss * ctrl_get_bss(struct wlantest *wt, int sock,
  123. u8 *cmd, size_t clen)
  124. {
  125. struct wlantest_bss *bss;
  126. u8 *pos;
  127. size_t len;
  128. pos = attr_get(cmd, clen, WLANTEST_ATTR_BSSID, &len);
  129. if (pos == NULL || len != ETH_ALEN) {
  130. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  131. return NULL;
  132. }
  133. bss = bss_find(wt, pos);
  134. if (bss == NULL) {
  135. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  136. return NULL;
  137. }
  138. return bss;
  139. }
  140. static struct wlantest_sta * ctrl_get_sta(struct wlantest *wt, int sock,
  141. u8 *cmd, size_t clen,
  142. struct wlantest_bss *bss)
  143. {
  144. struct wlantest_sta *sta;
  145. u8 *pos;
  146. size_t len;
  147. if (bss == NULL)
  148. return NULL;
  149. pos = attr_get(cmd, clen, WLANTEST_ATTR_STA_ADDR, &len);
  150. if (pos == NULL || len != ETH_ALEN) {
  151. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  152. return NULL;
  153. }
  154. sta = sta_find(bss, pos);
  155. if (sta == NULL) {
  156. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  157. return NULL;
  158. }
  159. return sta;
  160. }
  161. static struct wlantest_sta * ctrl_get_sta2(struct wlantest *wt, int sock,
  162. u8 *cmd, size_t clen,
  163. struct wlantest_bss *bss)
  164. {
  165. struct wlantest_sta *sta;
  166. u8 *pos;
  167. size_t len;
  168. if (bss == NULL)
  169. return NULL;
  170. pos = attr_get(cmd, clen, WLANTEST_ATTR_STA2_ADDR, &len);
  171. if (pos == NULL || len != ETH_ALEN) {
  172. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  173. return NULL;
  174. }
  175. sta = sta_find(bss, pos);
  176. if (sta == NULL) {
  177. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  178. return NULL;
  179. }
  180. return sta;
  181. }
  182. static void ctrl_list_bss(struct wlantest *wt, int sock)
  183. {
  184. u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos, *len;
  185. struct wlantest_bss *bss;
  186. pos = buf;
  187. WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
  188. pos += 4;
  189. WPA_PUT_BE32(pos, WLANTEST_ATTR_BSSID);
  190. pos += 4;
  191. len = pos; /* to be filled */
  192. pos += 4;
  193. dl_list_for_each(bss, &wt->bss, struct wlantest_bss, list) {
  194. if (pos + ETH_ALEN > buf + WLANTEST_CTRL_MAX_RESP_LEN)
  195. break;
  196. os_memcpy(pos, bss->bssid, ETH_ALEN);
  197. pos += ETH_ALEN;
  198. }
  199. WPA_PUT_BE32(len, pos - len - 4);
  200. ctrl_send(wt, sock, buf, pos - buf);
  201. }
  202. static void ctrl_list_sta(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
  203. {
  204. u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos, *len;
  205. struct wlantest_bss *bss;
  206. struct wlantest_sta *sta;
  207. bss = ctrl_get_bss(wt, sock, cmd, clen);
  208. if (bss == NULL)
  209. return;
  210. pos = buf;
  211. WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
  212. pos += 4;
  213. WPA_PUT_BE32(pos, WLANTEST_ATTR_STA_ADDR);
  214. pos += 4;
  215. len = pos; /* to be filled */
  216. pos += 4;
  217. dl_list_for_each(sta, &bss->sta, struct wlantest_sta, list) {
  218. if (pos + ETH_ALEN > buf + WLANTEST_CTRL_MAX_RESP_LEN)
  219. break;
  220. os_memcpy(pos, sta->addr, ETH_ALEN);
  221. pos += ETH_ALEN;
  222. }
  223. WPA_PUT_BE32(len, pos - len - 4);
  224. ctrl_send(wt, sock, buf, pos - buf);
  225. }
  226. static void ctrl_flush(struct wlantest *wt, int sock)
  227. {
  228. wpa_printf(MSG_DEBUG, "Drop all collected BSS data");
  229. bss_flush(wt);
  230. ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
  231. }
  232. static void ctrl_clear_sta_counters(struct wlantest *wt, int sock, u8 *cmd,
  233. size_t clen)
  234. {
  235. struct wlantest_bss *bss;
  236. struct wlantest_sta *sta;
  237. bss = ctrl_get_bss(wt, sock, cmd, clen);
  238. sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
  239. if (sta == NULL) {
  240. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  241. return;
  242. }
  243. os_memset(sta->counters, 0, sizeof(sta->counters));
  244. ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
  245. }
  246. static void ctrl_clear_bss_counters(struct wlantest *wt, int sock, u8 *cmd,
  247. size_t clen)
  248. {
  249. struct wlantest_bss *bss;
  250. bss = ctrl_get_bss(wt, sock, cmd, clen);
  251. if (bss == NULL) {
  252. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  253. return;
  254. }
  255. os_memset(bss->counters, 0, sizeof(bss->counters));
  256. ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
  257. }
  258. static void ctrl_clear_tdls_counters(struct wlantest *wt, int sock, u8 *cmd,
  259. size_t clen)
  260. {
  261. struct wlantest_bss *bss;
  262. struct wlantest_sta *sta;
  263. struct wlantest_sta *sta2;
  264. struct wlantest_tdls *tdls;
  265. bss = ctrl_get_bss(wt, sock, cmd, clen);
  266. sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
  267. sta2 = ctrl_get_sta2(wt, sock, cmd, clen, bss);
  268. if (sta == NULL || sta2 == NULL) {
  269. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  270. return;
  271. }
  272. dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
  273. if ((tdls->init == sta && tdls->resp == sta2) ||
  274. (tdls->init == sta2 && tdls->resp == sta))
  275. os_memset(tdls->counters, 0, sizeof(tdls->counters));
  276. }
  277. ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
  278. }
  279. static void ctrl_get_sta_counter(struct wlantest *wt, int sock, u8 *cmd,
  280. size_t clen)
  281. {
  282. u8 *addr;
  283. size_t addr_len;
  284. struct wlantest_bss *bss;
  285. struct wlantest_sta *sta;
  286. u32 counter;
  287. u8 buf[4 + 12], *end, *pos;
  288. bss = ctrl_get_bss(wt, sock, cmd, clen);
  289. sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
  290. if (sta == NULL)
  291. return;
  292. addr = attr_get(cmd, clen, WLANTEST_ATTR_STA_COUNTER, &addr_len);
  293. if (addr == NULL || addr_len != 4) {
  294. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  295. return;
  296. }
  297. counter = WPA_GET_BE32(addr);
  298. if (counter >= NUM_WLANTEST_STA_COUNTER) {
  299. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  300. return;
  301. }
  302. pos = buf;
  303. end = buf + sizeof(buf);
  304. WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
  305. pos += 4;
  306. pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
  307. sta->counters[counter]);
  308. ctrl_send(wt, sock, buf, pos - buf);
  309. }
  310. static void ctrl_get_bss_counter(struct wlantest *wt, int sock, u8 *cmd,
  311. size_t clen)
  312. {
  313. u8 *addr;
  314. size_t addr_len;
  315. struct wlantest_bss *bss;
  316. u32 counter;
  317. u8 buf[4 + 12], *end, *pos;
  318. bss = ctrl_get_bss(wt, sock, cmd, clen);
  319. if (bss == NULL)
  320. return;
  321. addr = attr_get(cmd, clen, WLANTEST_ATTR_BSS_COUNTER, &addr_len);
  322. if (addr == NULL || addr_len != 4) {
  323. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  324. return;
  325. }
  326. counter = WPA_GET_BE32(addr);
  327. if (counter >= NUM_WLANTEST_BSS_COUNTER) {
  328. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  329. return;
  330. }
  331. pos = buf;
  332. end = buf + sizeof(buf);
  333. WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
  334. pos += 4;
  335. pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
  336. bss->counters[counter]);
  337. ctrl_send(wt, sock, buf, pos - buf);
  338. }
  339. static void ctrl_get_tdls_counter(struct wlantest *wt, int sock, u8 *cmd,
  340. size_t clen)
  341. {
  342. u8 *addr;
  343. size_t addr_len;
  344. struct wlantest_bss *bss;
  345. struct wlantest_sta *sta;
  346. struct wlantest_sta *sta2;
  347. struct wlantest_tdls *tdls;
  348. u32 counter;
  349. u8 buf[4 + 12], *end, *pos;
  350. int found = 0;
  351. bss = ctrl_get_bss(wt, sock, cmd, clen);
  352. sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
  353. sta2 = ctrl_get_sta2(wt, sock, cmd, clen, bss);
  354. if (sta == NULL || sta2 == NULL) {
  355. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  356. return;
  357. }
  358. addr = attr_get(cmd, clen, WLANTEST_ATTR_TDLS_COUNTER, &addr_len);
  359. if (addr == NULL || addr_len != 4) {
  360. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  361. return;
  362. }
  363. counter = WPA_GET_BE32(addr);
  364. if (counter >= NUM_WLANTEST_TDLS_COUNTER) {
  365. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  366. return;
  367. }
  368. dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
  369. if (tdls->init == sta && tdls->resp == sta2) {
  370. found = 1;
  371. break;
  372. }
  373. }
  374. if (!found) {
  375. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  376. return;
  377. }
  378. pos = buf;
  379. end = buf + sizeof(buf);
  380. WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
  381. pos += 4;
  382. pos = attr_add_be32(pos, end, WLANTEST_ATTR_COUNTER,
  383. tdls->counters[counter]);
  384. ctrl_send(wt, sock, buf, pos - buf);
  385. }
  386. static void build_mgmt_hdr(struct ieee80211_mgmt *mgmt,
  387. struct wlantest_bss *bss, struct wlantest_sta *sta,
  388. int sender_ap, int stype)
  389. {
  390. os_memset(mgmt, 0, 24);
  391. mgmt->frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT, stype);
  392. if (sender_ap) {
  393. if (sta)
  394. os_memcpy(mgmt->da, sta->addr, ETH_ALEN);
  395. else
  396. os_memset(mgmt->da, 0xff, ETH_ALEN);
  397. os_memcpy(mgmt->sa, bss->bssid, ETH_ALEN);
  398. } else {
  399. os_memcpy(mgmt->da, bss->bssid, ETH_ALEN);
  400. os_memcpy(mgmt->sa, sta->addr, ETH_ALEN);
  401. }
  402. os_memcpy(mgmt->bssid, bss->bssid, ETH_ALEN);
  403. }
  404. static int ctrl_inject_auth(struct wlantest *wt, struct wlantest_bss *bss,
  405. struct wlantest_sta *sta, int sender_ap,
  406. enum wlantest_inject_protection prot)
  407. {
  408. struct ieee80211_mgmt mgmt;
  409. if (prot != WLANTEST_INJECT_NORMAL &&
  410. prot != WLANTEST_INJECT_UNPROTECTED)
  411. return -1; /* Authentication frame is never protected */
  412. if (sta == NULL)
  413. return -1; /* No broadcast Authentication frames */
  414. if (sender_ap)
  415. wpa_printf(MSG_INFO, "INJECT: Auth " MACSTR " -> " MACSTR,
  416. MAC2STR(bss->bssid), MAC2STR(sta->addr));
  417. else
  418. wpa_printf(MSG_INFO, "INJECT: Auth " MACSTR " -> " MACSTR,
  419. MAC2STR(sta->addr), MAC2STR(bss->bssid));
  420. build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_AUTH);
  421. mgmt.u.auth.auth_alg = host_to_le16(WLAN_AUTH_OPEN);
  422. mgmt.u.auth.auth_transaction = host_to_le16(1);
  423. mgmt.u.auth.status_code = host_to_le16(WLAN_STATUS_SUCCESS);
  424. return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 6,
  425. WLANTEST_INJECT_UNPROTECTED);
  426. }
  427. static int ctrl_inject_assocreq(struct wlantest *wt, struct wlantest_bss *bss,
  428. struct wlantest_sta *sta, int sender_ap,
  429. enum wlantest_inject_protection prot)
  430. {
  431. u8 *buf;
  432. struct ieee80211_mgmt *mgmt;
  433. int ret;
  434. if (prot != WLANTEST_INJECT_NORMAL &&
  435. prot != WLANTEST_INJECT_UNPROTECTED)
  436. return -1; /* Association Request frame is never protected */
  437. if (sta == NULL)
  438. return -1; /* No broadcast Association Request frames */
  439. if (sender_ap)
  440. return -1; /* No Association Request frame sent by AP */
  441. if (sta->assocreq_ies == NULL) {
  442. wpa_printf(MSG_INFO, "INJECT: No previous (Re)Association "
  443. "Request available for " MACSTR,
  444. MAC2STR(sta->addr));
  445. return -1;
  446. }
  447. wpa_printf(MSG_INFO, "INJECT: AssocReq " MACSTR " -> " MACSTR,
  448. MAC2STR(sta->addr), MAC2STR(bss->bssid));
  449. buf = os_malloc(sizeof(*mgmt) + sta->assocreq_ies_len);
  450. if (buf == NULL)
  451. return -1;
  452. mgmt = (struct ieee80211_mgmt *) buf;
  453. build_mgmt_hdr(mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_ASSOC_REQ);
  454. mgmt->u.assoc_req.capab_info = host_to_le16(sta->assocreq_capab_info);
  455. mgmt->u.assoc_req.listen_interval =
  456. host_to_le16(sta->assocreq_listen_int);
  457. os_memcpy(mgmt->u.assoc_req.variable, sta->assocreq_ies,
  458. sta->assocreq_ies_len);
  459. ret = wlantest_inject(wt, bss, sta, buf,
  460. 24 + 4 + sta->assocreq_ies_len,
  461. WLANTEST_INJECT_UNPROTECTED);
  462. os_free(buf);
  463. return ret;
  464. }
  465. static int ctrl_inject_reassocreq(struct wlantest *wt,
  466. struct wlantest_bss *bss,
  467. struct wlantest_sta *sta, int sender_ap,
  468. enum wlantest_inject_protection prot)
  469. {
  470. u8 *buf;
  471. struct ieee80211_mgmt *mgmt;
  472. int ret;
  473. if (prot != WLANTEST_INJECT_NORMAL &&
  474. prot != WLANTEST_INJECT_UNPROTECTED)
  475. return -1; /* Reassociation Request frame is never protected */
  476. if (sta == NULL)
  477. return -1; /* No broadcast Reassociation Request frames */
  478. if (sender_ap)
  479. return -1; /* No Reassociation Request frame sent by AP */
  480. if (sta->assocreq_ies == NULL) {
  481. wpa_printf(MSG_INFO, "INJECT: No previous (Re)Association "
  482. "Request available for " MACSTR,
  483. MAC2STR(sta->addr));
  484. return -1;
  485. }
  486. wpa_printf(MSG_INFO, "INJECT: ReassocReq " MACSTR " -> " MACSTR,
  487. MAC2STR(sta->addr), MAC2STR(bss->bssid));
  488. buf = os_malloc(sizeof(*mgmt) + sta->assocreq_ies_len);
  489. if (buf == NULL)
  490. return -1;
  491. mgmt = (struct ieee80211_mgmt *) buf;
  492. build_mgmt_hdr(mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_REASSOC_REQ);
  493. mgmt->u.reassoc_req.capab_info =
  494. host_to_le16(sta->assocreq_capab_info);
  495. mgmt->u.reassoc_req.listen_interval =
  496. host_to_le16(sta->assocreq_listen_int);
  497. os_memcpy(mgmt->u.reassoc_req.current_ap, bss->bssid, ETH_ALEN);
  498. os_memcpy(mgmt->u.reassoc_req.variable, sta->assocreq_ies,
  499. sta->assocreq_ies_len);
  500. ret = wlantest_inject(wt, bss, sta, buf,
  501. 24 + 10 + sta->assocreq_ies_len,
  502. WLANTEST_INJECT_UNPROTECTED);
  503. os_free(buf);
  504. return ret;
  505. }
  506. static int ctrl_inject_deauth(struct wlantest *wt, struct wlantest_bss *bss,
  507. struct wlantest_sta *sta, int sender_ap,
  508. enum wlantest_inject_protection prot)
  509. {
  510. struct ieee80211_mgmt mgmt;
  511. if (sender_ap) {
  512. if (sta)
  513. wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR " -> "
  514. MACSTR,
  515. MAC2STR(bss->bssid), MAC2STR(sta->addr));
  516. else
  517. wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR
  518. " -> broadcast", MAC2STR(bss->bssid));
  519. } else
  520. wpa_printf(MSG_INFO, "INJECT: Deauth " MACSTR " -> " MACSTR,
  521. MAC2STR(sta->addr), MAC2STR(bss->bssid));
  522. build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_DEAUTH);
  523. mgmt.u.deauth.reason_code = host_to_le16(WLAN_REASON_UNSPECIFIED);
  524. return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 2, prot);
  525. }
  526. static int ctrl_inject_disassoc(struct wlantest *wt, struct wlantest_bss *bss,
  527. struct wlantest_sta *sta, int sender_ap,
  528. enum wlantest_inject_protection prot)
  529. {
  530. struct ieee80211_mgmt mgmt;
  531. if (sender_ap) {
  532. if (sta)
  533. wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR " -> "
  534. MACSTR,
  535. MAC2STR(bss->bssid), MAC2STR(sta->addr));
  536. else
  537. wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR
  538. " -> broadcast", MAC2STR(bss->bssid));
  539. } else
  540. wpa_printf(MSG_INFO, "INJECT: Disassoc " MACSTR " -> " MACSTR,
  541. MAC2STR(sta->addr), MAC2STR(bss->bssid));
  542. build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_DISASSOC);
  543. mgmt.u.disassoc.reason_code = host_to_le16(WLAN_REASON_UNSPECIFIED);
  544. return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 2, prot);
  545. }
  546. static int ctrl_inject_saqueryreq(struct wlantest *wt,
  547. struct wlantest_bss *bss,
  548. struct wlantest_sta *sta, int sender_ap,
  549. enum wlantest_inject_protection prot)
  550. {
  551. struct ieee80211_mgmt mgmt;
  552. if (sta == NULL)
  553. return -1; /* No broadcast SA Query frames */
  554. if (sender_ap)
  555. wpa_printf(MSG_INFO, "INJECT: SA Query Request " MACSTR " -> "
  556. MACSTR, MAC2STR(bss->bssid), MAC2STR(sta->addr));
  557. else
  558. wpa_printf(MSG_INFO, "INJECT: SA Query Request " MACSTR " -> "
  559. MACSTR, MAC2STR(sta->addr), MAC2STR(bss->bssid));
  560. build_mgmt_hdr(&mgmt, bss, sta, sender_ap, WLAN_FC_STYPE_ACTION);
  561. mgmt.u.action.category = WLAN_ACTION_SA_QUERY;
  562. mgmt.u.action.u.sa_query_req.action = WLAN_SA_QUERY_REQUEST;
  563. mgmt.u.action.u.sa_query_req.trans_id[0] = 0x12;
  564. mgmt.u.action.u.sa_query_req.trans_id[1] = 0x34;
  565. os_memcpy(sender_ap ? sta->ap_sa_query_tr : sta->sta_sa_query_tr,
  566. mgmt.u.action.u.sa_query_req.trans_id,
  567. WLAN_SA_QUERY_TR_ID_LEN);
  568. return wlantest_inject(wt, bss, sta, (u8 *) &mgmt, 24 + 4, prot);
  569. }
  570. static void ctrl_inject(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
  571. {
  572. u8 *bssid, *sta_addr;
  573. struct wlantest_bss *bss;
  574. struct wlantest_sta *sta;
  575. int frame, sender_ap, prot;
  576. int ret = 0;
  577. bssid = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_BSSID);
  578. sta_addr = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_STA_ADDR);
  579. frame = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_FRAME);
  580. sender_ap = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_SENDER_AP);
  581. if (sender_ap < 0)
  582. sender_ap = 0;
  583. prot = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_PROTECTION);
  584. if (bssid == NULL || sta_addr == NULL || frame < 0 || prot < 0) {
  585. wpa_printf(MSG_INFO, "Invalid inject command parameters");
  586. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  587. return;
  588. }
  589. bss = bss_find(wt, bssid);
  590. if (bss == NULL) {
  591. wpa_printf(MSG_INFO, "BSS not found for inject command");
  592. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  593. return;
  594. }
  595. if (is_broadcast_ether_addr(sta_addr)) {
  596. if (!sender_ap) {
  597. wpa_printf(MSG_INFO, "Invalid broadcast inject "
  598. "command without sender_ap set");
  599. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  600. return;
  601. } sta = NULL;
  602. } else {
  603. sta = sta_find(bss, sta_addr);
  604. if (sta == NULL) {
  605. wpa_printf(MSG_INFO, "Station not found for inject "
  606. "command");
  607. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  608. return;
  609. }
  610. }
  611. switch (frame) {
  612. case WLANTEST_FRAME_AUTH:
  613. ret = ctrl_inject_auth(wt, bss, sta, sender_ap, prot);
  614. break;
  615. case WLANTEST_FRAME_ASSOCREQ:
  616. ret = ctrl_inject_assocreq(wt, bss, sta, sender_ap, prot);
  617. break;
  618. case WLANTEST_FRAME_REASSOCREQ:
  619. ret = ctrl_inject_reassocreq(wt, bss, sta, sender_ap, prot);
  620. break;
  621. case WLANTEST_FRAME_DEAUTH:
  622. ret = ctrl_inject_deauth(wt, bss, sta, sender_ap, prot);
  623. break;
  624. case WLANTEST_FRAME_DISASSOC:
  625. ret = ctrl_inject_disassoc(wt, bss, sta, sender_ap, prot);
  626. break;
  627. case WLANTEST_FRAME_SAQUERYREQ:
  628. ret = ctrl_inject_saqueryreq(wt, bss, sta, sender_ap, prot);
  629. break;
  630. default:
  631. wpa_printf(MSG_INFO, "Unsupported inject command frame %d",
  632. frame);
  633. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  634. return;
  635. }
  636. if (ret)
  637. wpa_printf(MSG_INFO, "Failed to inject frame");
  638. else
  639. wpa_printf(MSG_INFO, "Frame injected successfully");
  640. ctrl_send_simple(wt, sock, ret == 0 ? WLANTEST_CTRL_SUCCESS :
  641. WLANTEST_CTRL_FAILURE);
  642. }
  643. static void ctrl_version(struct wlantest *wt, int sock)
  644. {
  645. u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos;
  646. pos = buf;
  647. WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
  648. pos += 4;
  649. pos = attr_add_str(pos, buf + sizeof(buf), WLANTEST_ATTR_VERSION,
  650. VERSION_STR);
  651. ctrl_send(wt, sock, buf, pos - buf);
  652. }
  653. static void ctrl_add_passphrase(struct wlantest *wt, int sock, u8 *cmd,
  654. size_t clen)
  655. {
  656. u8 *passphrase;
  657. size_t len;
  658. struct wlantest_passphrase *p, *pa;
  659. u8 *bssid;
  660. passphrase = attr_get(cmd, clen, WLANTEST_ATTR_PASSPHRASE, &len);
  661. if (passphrase == NULL) {
  662. u8 *wepkey;
  663. char *key;
  664. enum wlantest_ctrl_cmd res;
  665. wepkey = attr_get(cmd, clen, WLANTEST_ATTR_WEPKEY, &len);
  666. if (wepkey == NULL) {
  667. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  668. return;
  669. }
  670. key = os_zalloc(len + 1);
  671. if (key == NULL) {
  672. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  673. return;
  674. }
  675. os_memcpy(key, wepkey, len);
  676. if (add_wep(wt, key) < 0)
  677. res = WLANTEST_CTRL_FAILURE;
  678. else
  679. res = WLANTEST_CTRL_SUCCESS;
  680. os_free(key);
  681. ctrl_send_simple(wt, sock, res);
  682. return;
  683. }
  684. if (len < 8 || len > 63) {
  685. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  686. return;
  687. }
  688. p = os_zalloc(sizeof(*p));
  689. if (p == NULL) {
  690. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  691. return;
  692. }
  693. os_memcpy(p->passphrase, passphrase, len);
  694. wpa_printf(MSG_INFO, "Add passphrase '%s'", p->passphrase);
  695. bssid = attr_get_macaddr(cmd, clen, WLANTEST_ATTR_BSSID);
  696. if (bssid) {
  697. os_memcpy(p->bssid, bssid, ETH_ALEN);
  698. wpa_printf(MSG_INFO, "Limit passphrase for BSSID " MACSTR,
  699. MAC2STR(p->bssid));
  700. }
  701. dl_list_for_each(pa, &wt->passphrase, struct wlantest_passphrase, list)
  702. {
  703. if (os_strcmp(p->passphrase, pa->passphrase) == 0 &&
  704. os_memcmp(p->bssid, pa->bssid, ETH_ALEN) == 0) {
  705. wpa_printf(MSG_INFO, "Passphrase was already known");
  706. os_free(p);
  707. p = NULL;
  708. break;
  709. }
  710. }
  711. if (p) {
  712. struct wlantest_bss *bss;
  713. dl_list_add(&wt->passphrase, &p->list);
  714. dl_list_for_each(bss, &wt->bss, struct wlantest_bss, list) {
  715. if (bssid &&
  716. os_memcmp(p->bssid, bss->bssid, ETH_ALEN) != 0)
  717. continue;
  718. bss_add_pmk_from_passphrase(bss, p->passphrase);
  719. }
  720. }
  721. ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
  722. }
  723. static void info_print_proto(char *buf, size_t len, int proto)
  724. {
  725. char *pos, *end;
  726. if (proto == 0) {
  727. os_snprintf(buf, len, "OPEN");
  728. return;
  729. }
  730. pos = buf;
  731. end = buf + len;
  732. if (proto & WPA_PROTO_WPA)
  733. pos += os_snprintf(pos, end - pos, "%sWPA",
  734. pos == buf ? "" : " ");
  735. if (proto & WPA_PROTO_RSN)
  736. pos += os_snprintf(pos, end - pos, "%sWPA2",
  737. pos == buf ? "" : " ");
  738. }
  739. static void info_print_cipher(char *buf, size_t len, int cipher)
  740. {
  741. char *pos, *end;
  742. if (cipher == 0) {
  743. os_snprintf(buf, len, "N/A");
  744. return;
  745. }
  746. pos = buf;
  747. end = buf + len;
  748. if (cipher & WPA_CIPHER_NONE)
  749. pos += os_snprintf(pos, end - pos, "%sNONE",
  750. pos == buf ? "" : " ");
  751. if (cipher & WPA_CIPHER_WEP40)
  752. pos += os_snprintf(pos, end - pos, "%sWEP40",
  753. pos == buf ? "" : " ");
  754. if (cipher & WPA_CIPHER_WEP104)
  755. pos += os_snprintf(pos, end - pos, "%sWEP104",
  756. pos == buf ? "" : " ");
  757. if (cipher & WPA_CIPHER_TKIP)
  758. pos += os_snprintf(pos, end - pos, "%sTKIP",
  759. pos == buf ? "" : " ");
  760. if (cipher & WPA_CIPHER_CCMP)
  761. pos += os_snprintf(pos, end - pos, "%sCCMP",
  762. pos == buf ? "" : " ");
  763. if (cipher & WPA_CIPHER_AES_128_CMAC)
  764. pos += os_snprintf(pos, end - pos, "%sBIP",
  765. pos == buf ? "" : " ");
  766. }
  767. static void info_print_key_mgmt(char *buf, size_t len, int key_mgmt)
  768. {
  769. char *pos, *end;
  770. if (key_mgmt == 0) {
  771. os_snprintf(buf, len, "N/A");
  772. return;
  773. }
  774. pos = buf;
  775. end = buf + len;
  776. if (key_mgmt & WPA_KEY_MGMT_IEEE8021X)
  777. pos += os_snprintf(pos, end - pos, "%sEAP",
  778. pos == buf ? "" : " ");
  779. if (key_mgmt & WPA_KEY_MGMT_PSK)
  780. pos += os_snprintf(pos, end - pos, "%sPSK",
  781. pos == buf ? "" : " ");
  782. if (key_mgmt & WPA_KEY_MGMT_WPA_NONE)
  783. pos += os_snprintf(pos, end - pos, "%sWPA-NONE",
  784. pos == buf ? "" : " ");
  785. if (key_mgmt & WPA_KEY_MGMT_FT_IEEE8021X)
  786. pos += os_snprintf(pos, end - pos, "%sFT-EAP",
  787. pos == buf ? "" : " ");
  788. if (key_mgmt & WPA_KEY_MGMT_FT_PSK)
  789. pos += os_snprintf(pos, end - pos, "%sFT-PSK",
  790. pos == buf ? "" : " ");
  791. if (key_mgmt & WPA_KEY_MGMT_IEEE8021X_SHA256)
  792. pos += os_snprintf(pos, end - pos, "%sEAP-SHA256",
  793. pos == buf ? "" : " ");
  794. if (key_mgmt & WPA_KEY_MGMT_PSK_SHA256)
  795. pos += os_snprintf(pos, end - pos, "%sPSK-SHA256",
  796. pos == buf ? "" : " ");
  797. }
  798. static void info_print_rsn_capab(char *buf, size_t len, int capab)
  799. {
  800. char *pos, *end;
  801. pos = buf;
  802. end = buf + len;
  803. if (capab & WPA_CAPABILITY_PREAUTH)
  804. pos += os_snprintf(pos, end - pos, "%sPREAUTH",
  805. pos == buf ? "" : " ");
  806. if (capab & WPA_CAPABILITY_NO_PAIRWISE)
  807. pos += os_snprintf(pos, end - pos, "%sNO_PAIRWISE",
  808. pos == buf ? "" : " ");
  809. if (capab & WPA_CAPABILITY_MFPR)
  810. pos += os_snprintf(pos, end - pos, "%sMFPR",
  811. pos == buf ? "" : " ");
  812. if (capab & WPA_CAPABILITY_MFPC)
  813. pos += os_snprintf(pos, end - pos, "%sMFPC",
  814. pos == buf ? "" : " ");
  815. if (capab & WPA_CAPABILITY_PEERKEY_ENABLED)
  816. pos += os_snprintf(pos, end - pos, "%sPEERKEY",
  817. pos == buf ? "" : " ");
  818. }
  819. static void info_print_state(char *buf, size_t len, int state)
  820. {
  821. switch (state) {
  822. case STATE1:
  823. os_strlcpy(buf, "NOT-AUTH", len);
  824. break;
  825. case STATE2:
  826. os_strlcpy(buf, "AUTH", len);
  827. break;
  828. case STATE3:
  829. os_strlcpy(buf, "AUTH+ASSOC", len);
  830. break;
  831. }
  832. }
  833. static void ctrl_info_sta(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
  834. {
  835. u8 *addr;
  836. size_t addr_len;
  837. struct wlantest_bss *bss;
  838. struct wlantest_sta *sta;
  839. enum wlantest_sta_info info;
  840. u8 buf[4 + 108], *end, *pos;
  841. char resp[100];
  842. bss = ctrl_get_bss(wt, sock, cmd, clen);
  843. sta = ctrl_get_sta(wt, sock, cmd, clen, bss);
  844. if (sta == NULL)
  845. return;
  846. addr = attr_get(cmd, clen, WLANTEST_ATTR_STA_INFO, &addr_len);
  847. if (addr == NULL || addr_len != 4) {
  848. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  849. return;
  850. }
  851. info = WPA_GET_BE32(addr);
  852. resp[0] = '\0';
  853. switch (info) {
  854. case WLANTEST_STA_INFO_PROTO:
  855. info_print_proto(resp, sizeof(resp), sta->proto);
  856. break;
  857. case WLANTEST_STA_INFO_PAIRWISE:
  858. info_print_cipher(resp, sizeof(resp), sta->pairwise_cipher);
  859. break;
  860. case WLANTEST_STA_INFO_KEY_MGMT:
  861. info_print_key_mgmt(resp, sizeof(resp), sta->key_mgmt);
  862. break;
  863. case WLANTEST_STA_INFO_RSN_CAPAB:
  864. info_print_rsn_capab(resp, sizeof(resp), sta->rsn_capab);
  865. break;
  866. case WLANTEST_STA_INFO_STATE:
  867. info_print_state(resp, sizeof(resp), sta->state);
  868. break;
  869. default:
  870. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  871. return;
  872. }
  873. pos = buf;
  874. end = buf + sizeof(buf);
  875. WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
  876. pos += 4;
  877. pos = attr_add_str(pos, end, WLANTEST_ATTR_INFO, resp);
  878. ctrl_send(wt, sock, buf, pos - buf);
  879. }
  880. static void ctrl_info_bss(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
  881. {
  882. u8 *addr;
  883. size_t addr_len;
  884. struct wlantest_bss *bss;
  885. enum wlantest_bss_info info;
  886. u8 buf[4 + 108], *end, *pos;
  887. char resp[100];
  888. bss = ctrl_get_bss(wt, sock, cmd, clen);
  889. if (bss == NULL)
  890. return;
  891. addr = attr_get(cmd, clen, WLANTEST_ATTR_BSS_INFO, &addr_len);
  892. if (addr == NULL || addr_len != 4) {
  893. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  894. return;
  895. }
  896. info = WPA_GET_BE32(addr);
  897. resp[0] = '\0';
  898. switch (info) {
  899. case WLANTEST_BSS_INFO_PROTO:
  900. info_print_proto(resp, sizeof(resp), bss->proto);
  901. break;
  902. case WLANTEST_BSS_INFO_PAIRWISE:
  903. info_print_cipher(resp, sizeof(resp), bss->pairwise_cipher);
  904. break;
  905. case WLANTEST_BSS_INFO_GROUP:
  906. info_print_cipher(resp, sizeof(resp), bss->group_cipher);
  907. break;
  908. case WLANTEST_BSS_INFO_GROUP_MGMT:
  909. info_print_cipher(resp, sizeof(resp), bss->mgmt_group_cipher);
  910. break;
  911. case WLANTEST_BSS_INFO_KEY_MGMT:
  912. info_print_key_mgmt(resp, sizeof(resp), bss->key_mgmt);
  913. break;
  914. case WLANTEST_BSS_INFO_RSN_CAPAB:
  915. info_print_rsn_capab(resp, sizeof(resp), bss->rsn_capab);
  916. break;
  917. default:
  918. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  919. return;
  920. }
  921. pos = buf;
  922. end = buf + sizeof(buf);
  923. WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS);
  924. pos += 4;
  925. pos = attr_add_str(pos, end, WLANTEST_ATTR_INFO, resp);
  926. ctrl_send(wt, sock, buf, pos - buf);
  927. }
  928. static void ctrl_send_(struct wlantest *wt, int sock, u8 *cmd, size_t clen)
  929. {
  930. struct wlantest_bss *bss;
  931. struct wlantest_sta *sta;
  932. u8 *bssid, *sta_addr;
  933. int prot;
  934. u8 *frame;
  935. size_t frame_len;
  936. int ret = 0;
  937. struct ieee80211_hdr *hdr;
  938. u16 fc;
  939. frame = attr_get(cmd, clen, WLANTEST_ATTR_FRAME, &frame_len);
  940. prot = attr_get_int(cmd, clen, WLANTEST_ATTR_INJECT_PROTECTION);
  941. if (frame == NULL || frame_len < 24 || prot < 0) {
  942. wpa_printf(MSG_INFO, "Invalid send command parameters");
  943. ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD);
  944. return;
  945. }
  946. hdr = (struct ieee80211_hdr *) frame;
  947. fc = le_to_host16(hdr->frame_control);
  948. switch (WLAN_FC_GET_TYPE(fc)) {
  949. case WLAN_FC_TYPE_MGMT:
  950. bssid = hdr->addr3;
  951. if (os_memcmp(hdr->addr2, hdr->addr3, ETH_ALEN) == 0)
  952. sta_addr = hdr->addr1;
  953. else
  954. sta_addr = hdr->addr2;
  955. break;
  956. case WLAN_FC_TYPE_DATA:
  957. switch (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) {
  958. case 0:
  959. bssid = hdr->addr3;
  960. sta_addr = hdr->addr2;
  961. break;
  962. case WLAN_FC_TODS:
  963. bssid = hdr->addr1;
  964. sta_addr = hdr->addr2;
  965. break;
  966. case WLAN_FC_FROMDS:
  967. bssid = hdr->addr2;
  968. sta_addr = hdr->addr1;
  969. break;
  970. default:
  971. wpa_printf(MSG_INFO, "Unsupported inject frame");
  972. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  973. return;
  974. }
  975. break;
  976. default:
  977. wpa_printf(MSG_INFO, "Unsupported inject frame");
  978. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  979. return;
  980. }
  981. bss = bss_find(wt, bssid);
  982. if (bss == NULL && prot != WLANTEST_INJECT_UNPROTECTED) {
  983. wpa_printf(MSG_INFO, "Unknown BSSID");
  984. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  985. return;
  986. }
  987. if (bss)
  988. sta = sta_find(bss, sta_addr);
  989. else
  990. sta = NULL;
  991. if (sta == NULL && prot != WLANTEST_INJECT_UNPROTECTED) {
  992. wpa_printf(MSG_INFO, "Unknown STA address");
  993. ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE);
  994. return;
  995. }
  996. ret = wlantest_inject(wt, bss, sta, frame, frame_len, prot);
  997. if (ret)
  998. wpa_printf(MSG_INFO, "Failed to inject frame");
  999. else
  1000. wpa_printf(MSG_INFO, "Frame injected successfully");
  1001. ctrl_send_simple(wt, sock, ret == 0 ? WLANTEST_CTRL_SUCCESS :
  1002. WLANTEST_CTRL_FAILURE);
  1003. }
  1004. static void ctrl_read(int sock, void *eloop_ctx, void *sock_ctx)
  1005. {
  1006. struct wlantest *wt = eloop_ctx;
  1007. u8 buf[WLANTEST_CTRL_MAX_CMD_LEN];
  1008. int len;
  1009. enum wlantest_ctrl_cmd cmd;
  1010. wpa_printf(MSG_EXCESSIVE, "New control interface message from %d",
  1011. sock);
  1012. len = recv(sock, buf, sizeof(buf), 0);
  1013. if (len < 0) {
  1014. wpa_printf(MSG_INFO, "recv(ctrl): %s", strerror(errno));
  1015. ctrl_disconnect(wt, sock);
  1016. return;
  1017. }
  1018. if (len == 0) {
  1019. ctrl_disconnect(wt, sock);
  1020. return;
  1021. }
  1022. if (len < 4) {
  1023. wpa_printf(MSG_INFO, "Too short control interface command "
  1024. "from %d", sock);
  1025. ctrl_disconnect(wt, sock);
  1026. return;
  1027. }
  1028. cmd = WPA_GET_BE32(buf);
  1029. wpa_printf(MSG_EXCESSIVE, "Control interface command %d from %d",
  1030. cmd, sock);
  1031. switch (cmd) {
  1032. case WLANTEST_CTRL_PING:
  1033. ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
  1034. break;
  1035. case WLANTEST_CTRL_TERMINATE:
  1036. ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS);
  1037. eloop_terminate();
  1038. break;
  1039. case WLANTEST_CTRL_LIST_BSS:
  1040. ctrl_list_bss(wt, sock);
  1041. break;
  1042. case WLANTEST_CTRL_LIST_STA:
  1043. ctrl_list_sta(wt, sock, buf + 4, len - 4);
  1044. break;
  1045. case WLANTEST_CTRL_FLUSH:
  1046. ctrl_flush(wt, sock);
  1047. break;
  1048. case WLANTEST_CTRL_CLEAR_STA_COUNTERS:
  1049. ctrl_clear_sta_counters(wt, sock, buf + 4, len - 4);
  1050. break;
  1051. case WLANTEST_CTRL_CLEAR_BSS_COUNTERS:
  1052. ctrl_clear_bss_counters(wt, sock, buf + 4, len - 4);
  1053. break;
  1054. case WLANTEST_CTRL_CLEAR_TDLS_COUNTERS:
  1055. ctrl_clear_tdls_counters(wt, sock, buf + 4, len - 4);
  1056. break;
  1057. case WLANTEST_CTRL_GET_STA_COUNTER:
  1058. ctrl_get_sta_counter(wt, sock, buf + 4, len - 4);
  1059. break;
  1060. case WLANTEST_CTRL_GET_BSS_COUNTER:
  1061. ctrl_get_bss_counter(wt, sock, buf + 4, len - 4);
  1062. break;
  1063. case WLANTEST_CTRL_GET_TDLS_COUNTER:
  1064. ctrl_get_tdls_counter(wt, sock, buf + 4, len - 4);
  1065. break;
  1066. case WLANTEST_CTRL_INJECT:
  1067. ctrl_inject(wt, sock, buf + 4, len - 4);
  1068. break;
  1069. case WLANTEST_CTRL_VERSION:
  1070. ctrl_version(wt, sock);
  1071. break;
  1072. case WLANTEST_CTRL_ADD_PASSPHRASE:
  1073. ctrl_add_passphrase(wt, sock, buf + 4, len - 4);
  1074. break;
  1075. case WLANTEST_CTRL_INFO_STA:
  1076. ctrl_info_sta(wt, sock, buf + 4, len - 4);
  1077. break;
  1078. case WLANTEST_CTRL_INFO_BSS:
  1079. ctrl_info_bss(wt, sock, buf + 4, len - 4);
  1080. break;
  1081. case WLANTEST_CTRL_SEND:
  1082. ctrl_send_(wt, sock, buf + 4, len - 4);
  1083. break;
  1084. default:
  1085. ctrl_send_simple(wt, sock, WLANTEST_CTRL_UNKNOWN_CMD);
  1086. break;
  1087. }
  1088. }
  1089. static void ctrl_connect(int sock, void *eloop_ctx, void *sock_ctx)
  1090. {
  1091. struct wlantest *wt = eloop_ctx;
  1092. int conn, i;
  1093. conn = accept(sock, NULL, NULL);
  1094. if (conn < 0) {
  1095. wpa_printf(MSG_INFO, "accept(ctrl): %s", strerror(errno));
  1096. return;
  1097. }
  1098. wpa_printf(MSG_MSGDUMP, "New control interface connection %d", conn);
  1099. for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
  1100. if (wt->ctrl_socks[i] < 0)
  1101. break;
  1102. }
  1103. if (i == MAX_CTRL_CONNECTIONS) {
  1104. wpa_printf(MSG_INFO, "No room for new control connection");
  1105. close(conn);
  1106. return;
  1107. }
  1108. wt->ctrl_socks[i] = conn;
  1109. eloop_register_read_sock(conn, ctrl_read, wt, NULL);
  1110. }
  1111. int ctrl_init(struct wlantest *wt)
  1112. {
  1113. struct sockaddr_un addr;
  1114. wt->ctrl_sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
  1115. if (wt->ctrl_sock < 0) {
  1116. wpa_printf(MSG_ERROR, "socket: %s", strerror(errno));
  1117. return -1;
  1118. }
  1119. os_memset(&addr, 0, sizeof(addr));
  1120. addr.sun_family = AF_UNIX;
  1121. os_strlcpy(addr.sun_path + 1, WLANTEST_SOCK_NAME,
  1122. sizeof(addr.sun_path) - 1);
  1123. if (bind(wt->ctrl_sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
  1124. wpa_printf(MSG_ERROR, "bind: %s", strerror(errno));
  1125. close(wt->ctrl_sock);
  1126. wt->ctrl_sock = -1;
  1127. return -1;
  1128. }
  1129. if (listen(wt->ctrl_sock, 5) < 0) {
  1130. wpa_printf(MSG_ERROR, "listen: %s", strerror(errno));
  1131. close(wt->ctrl_sock);
  1132. wt->ctrl_sock = -1;
  1133. return -1;
  1134. }
  1135. if (eloop_register_read_sock(wt->ctrl_sock, ctrl_connect, wt, NULL)) {
  1136. close(wt->ctrl_sock);
  1137. wt->ctrl_sock = -1;
  1138. return -1;
  1139. }
  1140. return 0;
  1141. }
  1142. void ctrl_deinit(struct wlantest *wt)
  1143. {
  1144. int i;
  1145. if (wt->ctrl_sock < 0)
  1146. return;
  1147. for (i = 0; i < MAX_CTRL_CONNECTIONS; i++) {
  1148. if (wt->ctrl_socks[i] >= 0) {
  1149. close(wt->ctrl_socks[i]);
  1150. eloop_unregister_read_sock(wt->ctrl_socks[i]);
  1151. wt->ctrl_socks[i] = -1;
  1152. }
  1153. }
  1154. eloop_unregister_read_sock(wt->ctrl_sock);
  1155. close(wt->ctrl_sock);
  1156. wt->ctrl_sock = -1;
  1157. }