wlantest: Add list_bss and list_sta commands

These can be used to list the current BSS and STA information
maintained in wlantest.

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;

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, "<BSSID> = 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;

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 */