From 9c034423514f58121bfbf9ea020168bb00fafd19 Mon Sep 17 00:00:00 2001
From: Jakob Olsson <jakob.olsson@iopsys.eu>
Date: Thu, 26 Nov 2020 14:48:11 +0100
Subject: [PATCH] map-agent: implement backhaul STA steering req/resp (Section
12)
---
src/core/agent.c | 158 +++++++++++++++++++++++++++++++--
src/core/agent.h | 8 +-
src/core/agent_map.c | 64 ++++++++++++-
src/core/agent_map.h | 2 +
src/core/agent_tlv_generator.c | 2 +
src/core/config.c | 55 +++++++++++-
src/core/config.h | 11 +++
7 files changed, 285 insertions(+), 15 deletions(-)
diff --git a/src/core/agent.c b/src/core/agent.c
index c3cae9a4d..92e731bdd 100644
--- a/src/core/agent.c
+++ b/src/core/agent.c
@@ -36,6 +36,9 @@
#include <wsc.h>
+#include <uci.h>
+
+
#include "utils/utils.h"
#include "map_module.h"
#include "utils.h"
@@ -54,6 +57,17 @@
static struct agent *this_agent;
static int signal_pending;
+static void bsta_steer_cb(struct uloop_timeout *t)
+{
+ struct netif_bk *bk = container_of(t, struct netif_bk, connect_timer);
+ uint8_t *bssid = NULL;
+
+ if (memcmp(bk->bsta_steer.prev_bssid, "\x00\x00\x00\x00\x00\x00", 6))
+ bssid = bk->bsta_steer.prev_bssid;
+
+ wifi_set_iface_bssid(bk->name, bssid);
+}
+
static void agent_terminate(void)
{
dbg("%s: called.\n", __func__);
@@ -219,6 +233,32 @@ static struct sta *find_sta_by_mac(struct agent *a, const unsigned char *mac)
return NULL;
}
+/* find bkhaul by hwaddr */
+struct netif_bk *find_bkhaul_by_ifname(struct agent *a, char *ifname)
+{
+ struct netif_bk *p;
+
+ list_for_each_entry(p, &a->bklist, list) {
+ if (!strncmp(p->name, ifname, sizeof(p->name)))
+ return p;
+ }
+
+ return NULL;
+}
+
+/* find bkhaul by hwaddr */
+struct netif_bk *find_bkhaul_by_bssid(struct agent *a, uint8_t *bssid)
+{
+ struct netif_bk *p;
+
+ list_for_each_entry(p, &a->bklist, list) {
+ if (!memcmp(p->bssid, bssid, 6))
+ return p;
+ }
+
+ return NULL;
+}
+
typedef void (*destructor)(void *);
#define delete_expired_entries(vif, type, h, l, ts_member, tmo, func, _nr) \
@@ -1639,6 +1679,88 @@ static void wifi_radio_event_handler(void *c, struct blob_attr *msg)
dbg("%s: Unhandled!!! TODO\n", __func__);
}
+static void wifi_bsta_event_handler(void *c, struct blob_attr *msg)
+{
+ char ifname[16] = {0}, event[16] = {0}, bssid_str[18] = {0};
+ struct agent *a = (struct agent *) c;
+ struct blob_attr *tb[3];
+ static const struct blobmsg_policy ev_attr[3] = {
+ [0] = { .name = "ifname", .type = BLOBMSG_TYPE_STRING },
+ [1] = { .name = "event", .type = BLOBMSG_TYPE_STRING },
+ [2] = { .name = "data", .type = BLOBMSG_TYPE_TABLE },
+ };
+ struct netif_bk *bk;
+ struct tlv_backhaul_steer_resp *p;
+ uint8_t bssid[6] = {0};
+ struct cmdu_cstruct *cmdu;
+
+ blobmsg_parse(ev_attr, 3, tb, blob_data(msg), blob_len(msg));
+
+ if (!tb[0] || !tb[1] || !tb[2])
+ return;
+
+ strncpy(ifname, blobmsg_data(tb[0]), sizeof(ifname));
+ strncpy(event, blobmsg_data(tb[1]), sizeof(event));
+
+ if (tb[2]) {
+ struct blob_attr *data[1];
+ static const struct blobmsg_policy data_attr[1] = {
+ [0] = { .name = "macaddr", .type = BLOBMSG_TYPE_STRING },
+ };
+
+ blobmsg_parse(data_attr, 1, data, blobmsg_data(tb[2]),
+ blobmsg_data_len(tb[2]));
+
+ strncpy(bssid_str, blobmsg_data(data[0]), sizeof(bssid_str));
+ }
+
+ bk = find_bkhaul_by_ifname(a, ifname);
+ if (!bk)
+ return;
+
+ cmdu = bk->bsta_steer.cmdu;
+ if (!cmdu)
+ goto fail_cmdu;
+
+ p = (struct tlv_backhaul_steer_resp *) extract_tlv_by_type(cmdu,
+ MAP_TLV_BACKHAUL_STEERING_RESPONSE);
+ if (!p)
+ goto fail_cmdu;
+
+ hwaddr_aton(bssid_str, bssid);
+
+ if (memcmp(bssid, p->bssid, 6)) {
+ struct tlv_error_code *p1;
+ uint8_t **tlvs;
+
+ p->res_code = 0x01;
+ p1 = (struct tlv_error_code *) calloc(1,
+ sizeof(struct tlv_error_code));
+ if (!p1)
+ goto fail_cmdu;
+
+ p1->tlv_type = MAP_TLV_ERROR_CODE;
+ p1->reason_code = 0x05;
+
+ cmdu->num_tlvs++;
+
+ tlvs = (uint8_t **)realloc(cmdu->tlvs,
+ cmdu->num_tlvs * sizeof(uint8_t *));
+ if (!tlvs) {
+ free(p1);
+ goto fail_cmdu;
+ }
+ cmdu->tlvs = tlvs;
+ cmdu->tlvs[cmdu->num_tlvs - 1] = (uint8_t *) p1;
+ }
+
+ uloop_timeout_cancel(&bk->connect_timer);
+ agent_send_cmdu(a, cmdu);
+fail_cmdu:
+ map_free_cmdu(cmdu);
+ bk->bsta_steer.cmdu = NULL;
+}
+
static void wifi_wps_creds_event_handler(void *c, struct blob_attr *msg)
{
char encryption[32] = {0}, ifname[16] = {0}, ssid[33] = {0},
@@ -1681,6 +1803,7 @@ static void ubus_wifi_event_handler(struct ubus_context *ctx,
{ "wifi.radio", wifi_radio_event_handler },
{ "wifi.iface", wifi_iface_event_handler },
{ "wps_credentials", wifi_wps_creds_event_handler },
+ { "wifi.bsta", wifi_bsta_event_handler }
};
str = blobmsg_format_json(msg, true);
@@ -2468,6 +2591,7 @@ static struct netif_bk *netif_alloc_bk(const char *ifname)
return NULL;
snprintf(n->name, 15, "%s", ifname);
+ n->connect_timer.cb = bsta_steer_cb;
return n;
}
@@ -2596,22 +2720,42 @@ static void parse_dot11ac(struct netif_fh *fh, struct blob_attr *arg)
fh->tx_spatial_streams = 0x08;
}
+static void parse_bk(struct ubus_request *req, int type,
+ struct blob_attr *msg)
+{
+ struct netif_bk *bk = (struct netif_bk *)req->priv;
+ char bssid[18] = { 0 };
+ struct blob_attr *tb[1];
+ static const struct blobmsg_policy ap_attr[1] = {
+ [0] = { .name = "bssid", .type = BLOBMSG_TYPE_STRING }
+ };
+
+ blobmsg_parse(ap_attr, 1, tb, blob_data(msg), blob_len(msg));
+
+ if (!tb[0])
+ return;
+
+ strncpy(bssid, blobmsg_data(tb[0]), 17);
+ hwaddr_aton(bssid, bk->bssid);
+}
+
static void parse_ap(struct ubus_request *req, int type,
struct blob_attr *msg)
{
struct wifi_radio_element *radio;
struct netif_fh *fh = (struct netif_fh *)req->priv;
char bssid[18] = { 0 }, ifname[16] = { 0 };
- struct blob_attr *tb[3];
- static const struct blobmsg_policy ap_attr[3] = {
+ struct blob_attr *tb[4];
+ static const struct blobmsg_policy ap_attr[4] = {
[0] = { .name = "ifname", .type = BLOBMSG_TYPE_STRING },
[1] = { .name = "bssid", .type = BLOBMSG_TYPE_STRING },
[2] = { .name = "capabilities", .type = BLOBMSG_TYPE_TABLE },
+ [3] = { .name = "channel", .type = BLOBMSG_TYPE_INT8 },
};
- blobmsg_parse(ap_attr, 3, tb, blob_data(msg), blob_len(msg));
+ blobmsg_parse(ap_attr, 4, tb, blob_data(msg), blob_len(msg));
- if (!tb[0] || !tb[1])
+ if (!tb[0] || !tb[1] || !tb[2])
return;
strncpy(ifname, blobmsg_data(tb[0]), 15);
@@ -2619,6 +2763,8 @@ static void parse_ap(struct ubus_request *req, int type,
strncpy(bssid, blobmsg_data(tb[1]), 17);
hwaddr_aton(bssid, fh->bssid);
+ fh->channel = blobmsg_get_u8(tb[2]);
+
if (tb[2]) {
struct blob_attr *data[2];
static const struct blobmsg_policy cap_attr[2] = {
@@ -2643,7 +2789,6 @@ static void parse_ap(struct ubus_request *req, int type,
return;
memcpy(radio->macaddr, fh->bssid, 6);
-
}
/* Initialize netif_fh/bk structs from ubus wifi objects */
@@ -2718,7 +2863,6 @@ static int agent_init_interfaces(struct agent *a)
}
list_for_each_entry(b, &cfg->bklist, list) {
-
wifi_object_t r_wobj = WIFI_OBJECT_INVALID;
wifi_object_t wobj = WIFI_OBJECT_INVALID;
const char *r_fmt = "wifi.radio.%s";
@@ -2755,6 +2899,8 @@ static int agent_init_interfaces(struct agent *a)
bn->cfg = b;
bn->agent = a;
list_add(&bn->list, &a->bklist);
+
+ ubus_call_object(a, wobj, "status", parse_bk, bn);
}
}
diff --git a/src/core/agent.h b/src/core/agent.h
index 013c6d4db..3f03cf13b 100644
--- a/src/core/agent.h
+++ b/src/core/agent.h
@@ -199,6 +199,12 @@ struct netif_bk {
wifi_object_t wifi;
wifi_object_t radio;
struct agent *agent;
+ struct {
+ uint8_t new_bssid[6];
+ uint8_t prev_bssid[6];
+ struct cmdu_cstruct *cmdu;
+ } bsta_steer;
+ struct uloop_timeout connect_timer;
};
struct agent_msg {
@@ -392,6 +398,7 @@ struct agent {
struct list_head pluginlist;
};
+struct netif_bk *find_bkhaul_by_bssid(struct agent *a, uint8_t *bssid);
extern int start_agent(void);
extern void stop_agent(struct agent *a);
@@ -400,7 +407,6 @@ extern int wifiagent_steer_sta(struct ubus_context *ctx, char *ifname,
unsigned char *sta, int bsscnt, unsigned char *bsss,
int optime);
-
extern int wifiagent_assoc_control_sta(char *fh_ifname, unsigned char *sta,
int enable, int tmo);
diff --git a/src/core/agent_map.c b/src/core/agent_map.c
index be223e572..9c798e0f0 100644
--- a/src/core/agent_map.c
+++ b/src/core/agent_map.c
@@ -22,6 +22,7 @@
#endif
#define TMP_WIFI_IFACE_MAX_NUM 16
+#define BSTA_STEER_TIMEOUT (7 * 1000)
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
@@ -34,7 +35,6 @@
#include <libubox/utils.h>
#include <libubus.h>
-
#include <uci.h>
#include "map_module.h"
@@ -1138,10 +1138,65 @@ int handle_hld_message(void *agent, struct cmdu_cstruct *cmdu)
return 0;
}
-int handle_backhaul_sta_steer_request(void *agent, struct cmdu_cstruct *cmdu)
+int handle_backhaul_sta_steer_request(void *agent,
+ struct cmdu_cstruct *rec_cmdu)
{
trace("%s: --->\n", __func__);
+ struct agent *a = (struct agent *) agent;
+ struct tlv_backhaul_steer_resp *p;
+ struct tlv_backhaul_steer_req *req;
+ struct cmdu_cstruct *cmdu;
+ struct netif_bk *bk;
+ int ret;
+
+ req = (struct tlv_backhaul_steer_req *) extract_tlv_by_type(rec_cmdu,
+ MAP_TLV_BACKHAUL_STEERING_REQUEST);
+ if (!req)
+ return -1;
+
+ bk = find_bkhaul_by_bssid(a, req->addr);
+ if (!bk)
+ return -1;
+
+ cmdu = (struct cmdu_cstruct *)calloc(1,
+ sizeof(struct cmdu_cstruct));
+ if (!cmdu) {
+ fprintf(stderr, "Out of memory!\n");
+ return -1;
+ }
+
+ cmdu->message_type = CMDU_BACKHAUL_STEER_RESPONSE;
+ memcpy(cmdu->origin, rec_cmdu->origin, 6);
+ cmdu->message_id = rec_cmdu->message_id;
+ strncpy(cmdu->intf_name, rec_cmdu->intf_name, sizeof(cmdu->intf_name));
+
+ p = calloc(1, sizeof(struct tlv_backhaul_steer_resp));
+ if (!p)
+ goto out_cmdu;
+
+ p->tlv_type = MAP_TLV_BACKHAUL_STEERING_RESPONSE;
+ memcpy(p->bssid, req->bssid, 6);
+ memcpy(p->addr, req->addr, 6);
+
+ cmdu->num_tlvs++;
+ cmdu->tlvs = (uint8_t **)calloc(cmdu->num_tlvs, sizeof(uint8_t *));
+ if (!cmdu->tlvs)
+ goto out_p;
+
+ cmdu->tlvs[0] = (uint8_t *) p;
+
+ if (bk->bsta_steer.cmdu)
+ map_free_cmdu(bk->bsta_steer.cmdu);
+ bk->bsta_steer.cmdu = cmdu;
+
+ wifi_set_iface_bssid(bk->name, req->bssid);
+ uloop_timeout_set(&bk->connect_timer, BSTA_STEER_TIMEOUT);
return 0;
+out_p:
+ free(p);
+out_cmdu:
+ free(cmdu);
+ return -1;
}
int handle_channel_scan_request(void *agent, struct cmdu_cstruct *cmdu)
@@ -1413,8 +1468,9 @@ int agent_send_cmdu(struct agent *a, struct cmdu_cstruct *cmdu)
hwaddr_ntoa(cmdu->origin, dst_addr);
blobmsg_add_string(&b, "dst_macaddr", dst_addr);
- trace("|%s:%d|cmdu:%s|dst:%s|\n", __func__, __LINE__,
- map_stringify_cmdu_type(cmdu->message_type), dst_addr);
+ trace("|%s:%d|cmdu:%s|egress:%s|dst:%s|\n", __func__, __LINE__,
+ map_stringify_cmdu_type(cmdu->message_type),
+ cmdu->intf_name, dst_addr);
if (cmdu->num_tlvs > 0) {
for (i = 0; i < cmdu->num_tlvs; i++) {
len = 0;
diff --git a/src/core/agent_map.h b/src/core/agent_map.h
index a2f9b30d3..c9159b0eb 100644
--- a/src/core/agent_map.h
+++ b/src/core/agent_map.h
@@ -3,6 +3,8 @@
#define AGENT_MAP_H
+uint8_t *extract_tlv_by_type(struct cmdu_cstruct *cmdu, uint8_t tlv_type);
+
extern bool is_cmdu_for_us(struct agent *a, uint16_t type);
extern int agent_handle_map_cmd(struct agent *a, char *data, int len);
diff --git a/src/core/agent_tlv_generator.c b/src/core/agent_tlv_generator.c
index dfa28546b..f85a1bca3 100644
--- a/src/core/agent_tlv_generator.c
+++ b/src/core/agent_tlv_generator.c
@@ -30,6 +30,8 @@
#include <map1905/maputils.h>
#include <wsc.h>
+#include <uci.h>
+
#include "map_module.h"
#include "utils.h"
#include "debug.h"
diff --git a/src/core/config.c b/src/core/config.c
index a9c229fac..5661d95c0 100644
--- a/src/core/config.c
+++ b/src/core/config.c
@@ -53,7 +53,7 @@
int verbose;
-static int set_value(struct uci_context *ctx, struct uci_package *pkg,
+int set_value(struct uci_context *ctx, struct uci_package *pkg,
struct uci_section *section, const char *key,
const char *value, enum uci_option_type type)
{
@@ -73,10 +73,9 @@ static int set_value(struct uci_context *ctx, struct uci_package *pkg,
return -1;
}
-static struct uci_section *config_get_iface_section(struct uci_context *ctx,
+struct uci_section *config_get_iface_section(struct uci_context *ctx,
struct uci_package *pkg, const char *type, const char *ifname)
{
-
struct uci_element *e;
struct uci_section *section;
@@ -96,6 +95,54 @@ static struct uci_section *config_get_iface_section(struct uci_context *ctx,
return NULL;
}
+struct uci_package *uci_load_pkg(struct uci_context **ctx, const char *config)
+{
+ struct uci_package *pkg;
+
+ if (!*ctx) {
+ *ctx = uci_alloc_context();
+ if (!*ctx)
+ return NULL;
+ }
+ if (uci_load(*ctx, config, &pkg) != UCI_OK) {
+ free(*ctx);
+ return NULL;
+ }
+
+ return pkg;
+}
+
+/* TODO: can it be generalized? */
+int wifi_set_iface_bssid(const char *ifname, uint8_t *bssid)
+{
+ struct uci_context *ctx = NULL;
+ struct uci_package *pkg;
+ struct uci_section *section;
+ char bssid_str[18] = {0};
+ int ret = -1;
+
+ pkg = uci_load_pkg(&ctx, "wireless");
+ if (!pkg)
+ return ret;
+
+ section = config_get_iface_section(ctx, pkg, "wifi-iface", ifname);
+ if (!section)
+ goto out_pkg;
+
+ if (bssid && !hwaddr_ntoa(bssid, bssid_str))
+ goto out_pkg;
+
+ ret = set_value(ctx, pkg, section, "bssid", bssid_str, UCI_TYPE_STRING);
+
+ uci_commit(ctx, &pkg, false);
+
+ uci_reload_services();
+out_pkg:
+ uci_unload(ctx, pkg);
+ uci_free_context(ctx);
+ return ret;
+}
+
int config_del_iface(const char *config, const char *type, const char *ifname)
{
struct uci_context *ctx;
@@ -492,7 +539,7 @@ static int ubus_call(const char *object, const char *method,
return UBUS_STATUS_OK;
}
-static bool uci_reload_services(void)
+bool uci_reload_services(void)
{
struct blob_buf bb;
diff --git a/src/core/config.h b/src/core/config.h
index 566d2b99a..f676ac23a 100644
--- a/src/core/config.h
+++ b/src/core/config.h
@@ -133,6 +133,17 @@ enum m2_process_status {
M2_PROCESS_TEARDOWN
};
+/* TODO: move to a uci_utils.c */
+int set_value(struct uci_context *ctx, struct uci_package *pkg,
+ struct uci_section *section, const char *key,
+ const char *value, enum uci_option_type type);
+struct uci_section *config_get_iface_section(struct uci_context *ctx,
+ struct uci_package *pkg, const char *type, const char *ifname);
+bool uci_reload_services(void);
+struct uci_package *uci_load_pkg(struct uci_context **ctx, const char *config);
+int wifi_set_iface_bssid(const char *ifname, uint8_t *bssid);
+
+/* END TODO */
int agent_config_init(struct agent_config *cfg);
int agent_config_reload(struct agent_config *cfg);
--
GitLab