add handler for sta_capabilities

src/core/cntlr_map.c

@@ -132,8 +132,11 @@ static bool ap_caps_info_from_tlv(struct controller *c,
 				struct tlv_ap_ht_cap *p = (struct tlv_ap_ht_cap *)tlv;
 
 				trace("\tradio_id: " MACFMT "\n", MAC2STR(p->radio_id));
-				trace("\tmax_tx_streams_supported: %02x\n", p->max_tx_streams_supported);
-				trace("\tmax_rx_streams_supported: %02x\n", p->max_rx_streams_supported);
+				trace("\tmax_tx_streams_supported: %d\n", p->max_tx_streams_supported);
+				trace("\tmax_tx_streams_supported: ");
+				print_bits(p->max_tx_streams_supported, 2, 2);
+				trace("\tmax_rx_streams_supported: ");
+				print_bits(p->max_rx_streams_supported, 2, 2);
 				trace("\tgi_20_support: %s\n", (p->gi_20_support ? "true" : "false"));
 				trace("\tgi_40_support: %s\n", (p->gi_40_support ? "true" : "false"));
 				trace("\tht_40_support: %s\n", (p->ht_40_support ? "true" : "false"));
@@ -144,10 +147,14 @@ static bool ap_caps_info_from_tlv(struct controller *c,
 				struct tlv_ap_vht_cap *p = (struct tlv_ap_vht_cap *)tlv;
 
 				trace("\tradio_id: " MACFMT "\n", MAC2STR(p->radio_id));
-				trace("\tvht_tx_mcs_supported: %04x\n", p->vht_tx_mcs_supported);
-				trace("\tvht_rx_mcs_supported: %04x\n", p->vht_rx_mcs_supported);
-				trace("\tmax_tx_streams_supported: %02x\n", p->max_tx_streams_supported);
-				trace("\tmax_rx_streams_supported: %02x\n", p->max_rx_streams_supported);
+				trace("\tvht_tx_mcs_supported: ");
+				print_bits(p->vht_tx_mcs_supported, 16, 2);
+				trace("\tvht_rx_mcs_supported: ");
+				print_bits(p->vht_rx_mcs_supported, 16, 2);
+				trace("\tmax_tx_streams_supported: ");
+				print_bits(p->max_tx_streams_supported, 3, 3);
+				trace("\tmax_rx_streams_supported: ");
+				print_bits(p->max_rx_streams_supported, 3, 3);
 				trace("\tgi_80_support: %s\n", (p->gi_80_support ? "true" : "false"));
 				trace("\tgi_160_support: %s\n", (p->gi_160_support ? "true" : "false"));
 				trace("\tvht160_support: %s\n", (p->vht_160_support ? "true" : "false"));
@@ -194,9 +201,65 @@ int handle_oper_channel_report(void *cntlr, struct cmdu_cstruct *cmdu)
 	return 0;
 }
 
+static bool sta_caps_info_from_tlv(struct controller *c,
+					struct cmdu_cstruct *cmdu_data)
+{
+	int i, j, k;
+	int radio_index, bss_index;
+	uint8_t *tlv = NULL;
+
+	for (i = 0; i < cmdu_data->num_tlvs; i++) {
+		tlv = cmdu_data->tlvs[i];
+		trace("CMDU type: %s\n", map_stringify_tlv_type(*tlv));
+		switch (*tlv) {
+		case MAP_TLV_CLIENT_INFO:
+			{
+				struct tlv_client_info *p = (struct tlv_client_info *)tlv;
+
+				trace("\tbssid: " MACFMT "\n", MAC2STR(p->bssid));
+				trace("\tclient_addr: " MACFMT "\n", MAC2STR(p->client_addr));
+				break;
+			}
+		case MAP_TLV_CLIENT_CAPABILITY_REPORT:
+			{
+				struct tlv_client_cap_report *p = (struct tlv_client_cap_report *)tlv;
+				char *frame;
+				int offset = 1 + 2 + 1;
+
+				frame = calloc(1, (2 * (p->tlv_len - offset)) + 1);
+				if (!frame)
+					continue;
+
+				btostr(p->frame_body + offset, p->tlv_len - offset, frame);
+
+				trace("\tresult_code: 0x%02x\n", p->result_code);
+				trace("\tframe: 0x%s\n", frame);
+				break;
+			}
+		case MAP_TLV_ERROR_CODE:
+			{
+				struct tlv_error_code *p = (struct tlv_error_code *)tlv;
+
+				trace("\treason_code: 0x%02x\n", p->reason_code);
+				if (p->reason_code == 0x02)
+					trace("\tclient_addr: " MACFMT "\n", MAC2STR(p->addr));
+
+				break;
+			}
+		default:
+			fprintf(stdout, "unknown TLV in CMDU:|%s|", map_stringify_cmdu_type(cmdu_data->message_type));
+			break;
+		}
+		trace("\n");
+	}
+
+	return 0;
+}
+
 int handle_sta_caps_report(void *cntlr, struct cmdu_cstruct *cmdu)
 {
 	trace("received sta caps report!\n");
+	sta_caps_info_from_tlv(cntlr, cmdu);
 	return 0;
 }
 

src/core/cntlr_ubus.c

@@ -36,16 +36,29 @@
 
 enum {
 	AP_POLICY_AGENT,
-	//AP_POLICY_BSSID,
+	//AP_POLICY_BSSID, /* TODO: filter on cntlr side based on bssid */
 	__AP_POLICY_MAX,
 };
 
 static const struct blobmsg_policy ap_caps_policy_params[__AP_POLICY_MAX] = {
-	[AP_POLICY_AGENT] = { .name = "agent", .type = BLOBMSG_TYPE_STRING }, // agent = ieee1905 AL macaddress of an agent, agent = "ff:ff:ff:ff:ff:ff" means all agents
-	//[AP_POLICY_BSSID] = { .name = "bssid", .type = BLOBMSG_TYPE_STRING }
+	[AP_POLICY_AGENT] = { .name = "agent", .type = BLOBMSG_TYPE_STRING },
+	//[AP_POLICY_BSSID] = { .name = "bssid", .type = BLOBMSG_TYPE_STRING } /* TODO: filter on cntlr side based on bssid */
 };
 
 
+enum {
+	STA_POLICY_AGENT,
+	STA_POLICY_STA,
+	STA_POLICY_BSSID,
+	__STA_POLICY_MAX,
+};
+
+static const struct blobmsg_policy sta_caps_policy_params[__STA_POLICY_MAX] = {
+	[STA_POLICY_AGENT] = { .name = "agent", .type = BLOBMSG_TYPE_STRING },
+	[STA_POLICY_STA] = { .name = "sta", .type = BLOBMSG_TYPE_STRING },
+	[STA_POLICY_BSSID] = { .name = "bssid", .type = BLOBMSG_TYPE_STRING }
+};
+
 static void send_cmdu_cb(struct ubus_request *req,
 				int type, struct blob_attr *msg)
 {
@@ -110,8 +123,8 @@ static int send_cmdu(struct controller *c,
 	hwaddr_ntoa(cmdu_data->origin, dst_addr);
 	blobmsg_add_string(&b, "dst_macaddr", dst_addr);
 
-	fprintf(stdout, "|%s:%d|cmdu:%s|dst:%s|\n", __func__, __LINE__,
-			map_stringify_cmdu_type(cmdu_data->message_type), dst_addr);
+	fprintf(stdout, "|%s:%d|cmdu:%s|dst:%s|num_tlvs:%d|\n", __func__, __LINE__,
+			map_stringify_cmdu_type(cmdu_data->message_type), dst_addr, cmdu_data->num_tlvs);
 
 	if (cmdu_data->num_tlvs > 0) {
 		for (i = 0; i < cmdu_data->num_tlvs; i++) {
@@ -293,7 +306,7 @@ static int cntlr_ap_caps(struct ubus_context *ctx, struct ubus_object *obj,
 {
 	struct blob_attr *tb[__AP_POLICY_MAX];
 	struct controller *c = container_of(obj, struct controller, obj);
-	char mac[18] = {0};
+	char agent[18] = {0};
 	struct cmdu_cstruct *cmdu_data;
 	uint32_t total_bss = 0;
 	uint32_t k = 0;
@@ -309,9 +322,8 @@ static int cntlr_ap_caps(struct ubus_context *ctx, struct ubus_object *obj,
 
 
 	if (tb[AP_POLICY_AGENT]) {
-		strncpy(mac, blobmsg_data(tb[AP_POLICY_AGENT]), sizeof(mac) - 1);
-		printf("mac = %s\n", mac);
-		if (!hwaddr_aton(mac, cmdu_data->origin))
+		strncpy(agent, blobmsg_data(tb[AP_POLICY_AGENT]), sizeof(agent) - 1);
+		if (!hwaddr_aton(agent, cmdu_data->origin))
 			return UBUS_STATUS_UNKNOWN_ERROR;
 	}
 
@@ -327,15 +339,87 @@ static int cntlr_ap_caps(struct ubus_context *ctx, struct ubus_object *obj,
 	return 0;
 }
 
+static int cntlr_sta_caps(struct ubus_context *ctx, struct ubus_object *obj,
+			struct ubus_request_data *req, const char *method,
+			struct blob_attr *msg)
+{
+	struct blob_attr *tb[__STA_POLICY_MAX];
+	uint8_t hw_bssid[6] = {0}, hw_sta[6] = {0};
+	struct tlv_client_info *p;
+	struct controller *c = container_of(obj, struct controller, obj);
+	char sta[18] = {0}, agent[18] = {0}, bssid[18] = {0};
+	struct cmdu_cstruct *cmdu_data;
+
+	blobmsg_parse(sta_caps_policy_params, __STA_POLICY_MAX, tb, blob_data(msg), blob_len(msg));
+
+	if (!tb[STA_POLICY_STA] || !tb[STA_POLICY_BSSID]) {
+		fprintf(stderr, "STA Capability Query: must provide STA and BSSID\n");
+		return UBUS_STATUS_INVALID_ARGUMENT;
+	}
+
+	strncpy(sta, blobmsg_data(tb[STA_POLICY_STA]), sizeof(sta) - 1);
+	strncpy(bssid, blobmsg_data(tb[STA_POLICY_BSSID]), sizeof(bssid) - 1);
+
+	cmdu_data = (struct cmdu_cstruct *)calloc(1, sizeof(struct cmdu_cstruct));
+	if (!cmdu_data) {
+		fprintf(stderr, "failed to malloc cmdu\n");
+		return UBUS_STATUS_UNKNOWN_ERROR;
+	}
+
+	if (tb[AP_POLICY_AGENT]) {
+		strncpy(agent, blobmsg_data(tb[AP_POLICY_AGENT]), sizeof(agent) - 1);
+		if (!hwaddr_aton(agent, cmdu_data->origin))
+			return UBUS_STATUS_UNKNOWN_ERROR;
+	}
+
+	// TODO: ff:ff:ff:ff:ff:ff = send to all agents
+
+	cmdu_data->message_type = CMDU_CLIENT_CAPABILITY_QUERY;
+	cmdu_data->message_id = 1;
+
+	p = calloc(1, sizeof(struct tlv_client_info));
+	if (!p) {
+		fprintf(stderr, "failed to malloc cmdu\n");
+		return UBUS_STATUS_UNKNOWN_ERROR;
+	}
+
+	if (!hwaddr_aton(sta, hw_sta)) {
+			fprintf(stderr, "STA Capability Query: provide STA address in " \
+				"format 11:22:33...\n");
+		return UBUS_STATUS_INVALID_ARGUMENT;
+	}
+
+	if (!hwaddr_aton(bssid, hw_bssid)) {
+			fprintf(stderr, "STA Capability Query: provide BSSID address in " \
+					"format 11:22:33...\n");
+		return UBUS_STATUS_INVALID_ARGUMENT;
+	}
+
+	p->tlv_type = MAP_TLV_CLIENT_INFO;
+	memcpy(p->bssid, hw_bssid, 6);
+	memcpy(p->client_addr, hw_sta, 6);
+
+	cmdu_data->num_tlvs = 1;
+	cmdu_data->tlvs = (uint8_t **)calloc(cmdu_data->num_tlvs, sizeof(uint8_t *));
+
+	if (cmdu_data->tlvs)
+			cmdu_data->tlvs[0] = (uint8_t *)p;
+
+	send_cmdu(c, cmdu_data);
+
+	return 0;
+}
+
 int cntlr_publish_object(struct controller *c, const char *objname)
 {
 	struct ubus_object *obj;
 	struct ubus_object_type *obj_type;
 	struct ubus_method *obj_methods;
-	struct ubus_method m[3] = {
+	struct ubus_method m[4] = {
 		UBUS_METHOD_NOARG("status", cntlr_status),
 		UBUS_METHOD_NOARG("req_cap", cntlr_req_cap),
 		UBUS_METHOD("ap_caps", cntlr_ap_caps, ap_caps_policy_params),
+		UBUS_METHOD("sta_caps", cntlr_sta_caps, sta_caps_policy_params),
 	};
 	int num_methods = ARRAY_SIZE(m);
 	int ret;

src/utils/utils.h

@@ -28,6 +28,16 @@
 #define MAC2STR(_m)	(_m)[0], (_m)[1], (_m)[2], (_m)[3], (_m)[4], (_m)[5]
 #endif
 
+#define print_bits(x, len, s) \
+do { \
+        unsigned long long a__ = (x); \
+        size_t bits__ = sizeof(x) * len; \
+        while(bits__--) { \
+			putchar(a__ & (1 << bits__) ? '1' : '0'); \
+			if (!(bits__ % s)) putchar(' '); \
+		} \
+        putchar('\n'); \
+} while(0)
 
 bool match_oui0(unsigned char *oui, unsigned char *hwaddr, int ouis);
 unsigned char *hwaddr_aton(const char *macstr, unsigned char *mac);