diff --git a/wlantest/ctrl.c b/wlantest/ctrl.c index 3a292d64c..3f880295b 100644 --- a/wlantest/ctrl.c +++ b/wlantest/ctrl.c @@ -21,6 +21,34 @@ #include "wlantest_ctrl.h" +static u8 * attr_get(u8 *buf, size_t buflen, enum wlantest_ctrl_attr attr, + size_t *len) +{ + u8 *pos = buf; + + while (pos + 8 <= buf + buflen) { + enum wlantest_ctrl_attr a; + size_t alen; + a = WPA_GET_BE32(pos); + pos += 4; + alen = WPA_GET_BE32(pos); + pos += 4; + if (pos + alen > buf + buflen) { + wpa_printf(MSG_DEBUG, "Invalid control message " + "attribute"); + return NULL; + } + if (a == attr) { + *len = alen; + return pos; + } + pos += alen; + } + + return NULL; +} + + static void ctrl_disconnect(struct wlantest *wt, int sock) { int i; @@ -37,15 +65,87 @@ static void ctrl_disconnect(struct wlantest *wt, int sock) } +static void ctrl_send(struct wlantest *wt, int sock, const u8 *buf, + size_t len) +{ + if (send(sock, buf, len, 0) < 0) { + wpa_printf(MSG_INFO, "send(ctrl): %s", strerror(errno)); + ctrl_disconnect(wt, sock); + } +} + + static void ctrl_send_simple(struct wlantest *wt, int sock, enum wlantest_ctrl_cmd cmd) { u8 buf[4]; WPA_PUT_BE32(buf, cmd); - if (send(sock, buf, sizeof(buf), 0) < 0) { - wpa_printf(MSG_INFO, "send(ctrl): %s", strerror(errno)); - ctrl_disconnect(wt, sock); + ctrl_send(wt, sock, buf, sizeof(buf)); +} + + +static void ctrl_list_bss(struct wlantest *wt, int sock) +{ + u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos, *len; + struct wlantest_bss *bss; + + pos = buf; + WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS); + pos += 4; + WPA_PUT_BE32(pos, WLANTEST_ATTR_BSSID); + pos += 4; + len = pos; /* to be filled */ + pos += 4; + + dl_list_for_each(bss, &wt->bss, struct wlantest_bss, list) { + if (pos + ETH_ALEN > buf + WLANTEST_CTRL_MAX_RESP_LEN) + break; + os_memcpy(pos, bss->bssid, ETH_ALEN); + pos += ETH_ALEN; } + + WPA_PUT_BE32(len, pos - len - 4); + ctrl_send(wt, sock, buf, pos - buf); +} + + +static void ctrl_list_sta(struct wlantest *wt, int sock, u8 *cmd, size_t clen) +{ + u8 buf[WLANTEST_CTRL_MAX_RESP_LEN], *pos, *len; + u8 *bssid; + size_t bssid_len; + struct wlantest_bss *bss; + struct wlantest_sta *sta; + + bssid = attr_get(cmd, clen, WLANTEST_ATTR_BSSID, &bssid_len); + if (bssid == NULL || bssid_len != ETH_ALEN) { + ctrl_send_simple(wt, sock, WLANTEST_CTRL_INVALID_CMD); + return; + } + + bss = bss_get(wt, bssid); + if (bss == NULL) { + ctrl_send_simple(wt, sock, WLANTEST_CTRL_FAILURE); + return; + } + + pos = buf; + WPA_PUT_BE32(pos, WLANTEST_CTRL_SUCCESS); + pos += 4; + WPA_PUT_BE32(pos, WLANTEST_ATTR_STA_ADDR); + pos += 4; + len = pos; /* to be filled */ + pos += 4; + + dl_list_for_each(sta, &bss->sta, struct wlantest_sta, list) { + if (pos + ETH_ALEN > buf + WLANTEST_CTRL_MAX_RESP_LEN) + break; + os_memcpy(pos, sta->addr, ETH_ALEN); + pos += ETH_ALEN; + } + + WPA_PUT_BE32(len, pos - len - 4); + ctrl_send(wt, sock, buf, pos - buf); } @@ -87,6 +187,12 @@ static void ctrl_read(int sock, void *eloop_ctx, void *sock_ctx) ctrl_send_simple(wt, sock, WLANTEST_CTRL_SUCCESS); eloop_terminate(); break; + case WLANTEST_CTRL_LIST_BSS: + ctrl_list_bss(wt, sock); + break; + case WLANTEST_CTRL_LIST_STA: + ctrl_list_sta(wt, sock, buf + 4, len - 4); + break; default: ctrl_send_simple(wt, sock, WLANTEST_CTRL_UNKNOWN_CMD); break; diff --git a/wlantest/wlantest_cli.c b/wlantest/wlantest_cli.c index 024626ce1..7ba5ff175 100644 --- a/wlantest/wlantest_cli.c +++ b/wlantest/wlantest_cli.c @@ -19,34 +19,74 @@ #include "wlantest_ctrl.h" -static int cmd_simple(int s, enum wlantest_ctrl_cmd cmd) +static u8 * attr_get(u8 *buf, size_t buflen, enum wlantest_ctrl_attr attr, + size_t *len) { - char buf[4]; - int res; - enum wlantest_ctrl_cmd resp; + u8 *pos = buf; - WPA_PUT_BE32(buf, cmd); - if (send(s, buf, 4, 0) < 0) + while (pos + 8 <= buf + buflen) { + enum wlantest_ctrl_attr a; + size_t alen; + a = WPA_GET_BE32(pos); + pos += 4; + alen = WPA_GET_BE32(pos); + pos += 4; + if (pos + alen > buf + buflen) { + printf("Invalid control message attribute\n"); + return NULL; + } + if (a == attr) { + *len = alen; + return pos; + } + pos += alen; + } + + return NULL; +} + + +static int cmd_send_and_recv(int s, const u8 *cmd, size_t cmd_len, + u8 *resp, size_t max_resp_len) +{ + int res; + enum wlantest_ctrl_cmd cmd_resp; + + if (send(s, cmd, cmd_len, 0) < 0) return -1; - res = recv(s, buf, sizeof(buf), 0); + res = recv(s, resp, max_resp_len, 0); if (res < 4) return -1; - resp = WPA_GET_BE32(buf); - if (resp == WLANTEST_CTRL_SUCCESS) - printf("OK\n"); - else if (resp == WLANTEST_CTRL_FAILURE) - printf("FAIL\n"); - else if (resp == WLANTEST_CTRL_UNKNOWN_CMD) - printf("Unknown command\n"); + cmd_resp = WPA_GET_BE32(resp); + if (cmd_resp == WLANTEST_CTRL_SUCCESS) + return res; - return resp == WLANTEST_CTRL_SUCCESS ? 0 : -1; + if (cmd_resp == WLANTEST_CTRL_UNKNOWN_CMD) + printf("Unknown command\n"); + else if (cmd_resp == WLANTEST_CTRL_INVALID_CMD) + printf("Invalid command\n"); + + return -1; +} + + +static int cmd_simple(int s, enum wlantest_ctrl_cmd cmd) +{ + u8 buf[4]; + int res; + WPA_PUT_BE32(buf, cmd); + res = cmd_send_and_recv(s, buf, sizeof(buf), buf, sizeof(buf)); + return res < 0 ? -1 : 0; } static int cmd_ping(int s, int argc, char *argv[]) { - return cmd_simple(s, WLANTEST_CTRL_PING) == 0; + int res = cmd_simple(s, WLANTEST_CTRL_PING); + if (res == 0) + printf("PONG\n"); + return res == 0; } @@ -56,6 +96,73 @@ static int cmd_terminate(int s, int argc, char *argv[]) } +static int cmd_list_bss(int s, int argc, char *argv[]) +{ + u8 resp[WLANTEST_CTRL_MAX_RESP_LEN]; + u8 buf[4]; + u8 *bssid; + size_t len; + int rlen, i; + + WPA_PUT_BE32(buf, WLANTEST_CTRL_LIST_BSS); + rlen = cmd_send_and_recv(s, buf, sizeof(buf), resp, sizeof(resp)); + if (rlen < 0) + return -1; + + bssid = attr_get(resp + 4, rlen - 4, WLANTEST_ATTR_BSSID, &len); + if (bssid == NULL) + return -1; + + for (i = 0; i < len / ETH_ALEN; i++) + printf(MACSTR " ", MAC2STR(bssid + ETH_ALEN * i)); + printf("\n"); + + return 0; +} + + +static int cmd_list_sta(int s, int argc, char *argv[]) +{ + u8 resp[WLANTEST_CTRL_MAX_RESP_LEN]; + u8 buf[100], *pos; + u8 *addr; + size_t len; + int rlen, i; + + if (argc < 1) { + printf("list_sta needs one argument: BSSID\n"); + return -1; + } + + pos = buf; + WPA_PUT_BE32(pos, WLANTEST_CTRL_LIST_STA); + pos += 4; + WPA_PUT_BE32(pos, WLANTEST_ATTR_BSSID); + pos += 4; + WPA_PUT_BE32(pos, ETH_ALEN); + pos += 4; + if (hwaddr_aton(argv[0], pos) < 0) { + printf("Invalid BSSID '%s'\n", argv[0]); + return -1; + } + pos += ETH_ALEN; + + rlen = cmd_send_and_recv(s, buf, pos - buf, resp, sizeof(resp)); + if (rlen < 0) + return -1; + + addr = attr_get(resp + 4, rlen - 4, WLANTEST_ATTR_STA_ADDR, &len); + if (addr == NULL) + return -1; + + for (i = 0; i < len / ETH_ALEN; i++) + printf(MACSTR " ", MAC2STR(addr + ETH_ALEN * i)); + printf("\n"); + + return 0; +} + + struct wlantest_cli_cmd { const char *cmd; int (*handler)(int s, int argc, char *argv[]); @@ -65,6 +172,8 @@ struct wlantest_cli_cmd { static const struct wlantest_cli_cmd wlantest_cli_commands[] = { { "ping", cmd_ping, "= test connection to wlantest" }, { "terminate", cmd_terminate, "= terminate wlantest" }, + { "list_bss", cmd_list_bss, "= get BSS list" }, + { "list_sta", cmd_list_sta, " = get STA list" }, { NULL, NULL, NULL } }; @@ -95,7 +204,6 @@ static int ctrl_command(int s, int argc, char *argv[]) os_strlen(argv[0])) == 0) { printf(" %s", cmd->cmd); } - cmd++; } printf("\n"); ret = 1; diff --git a/wlantest/wlantest_ctrl.h b/wlantest/wlantest_ctrl.h index d34bc88d3..85db87ee1 100644 --- a/wlantest/wlantest_ctrl.h +++ b/wlantest/wlantest_ctrl.h @@ -22,9 +22,17 @@ enum wlantest_ctrl_cmd { WLANTEST_CTRL_SUCCESS, WLANTEST_CTRL_FAILURE, + WLANTEST_CTRL_INVALID_CMD, WLANTEST_CTRL_UNKNOWN_CMD, WLANTEST_CTRL_PING, WLANTEST_CTRL_TERMINATE, + WLANTEST_CTRL_LIST_BSS, + WLANTEST_CTRL_LIST_STA, +}; + +enum wlantest_ctrl_attr { + WLANTEST_ATTR_BSSID, + WLANTEST_ATTR_STA_ADDR, }; #endif /* WLANTEST_CTRL_H */