diff --git a/src/agent.c b/src/agent.c
index f8f8b262362f89a907e05cb0543cf74490f20661..37eb8f29830a9ae629b23141970e96609167337e 100644
--- a/src/agent.c
+++ b/src/agent.c
@@ -1513,7 +1513,8 @@ static void wifi_sta_periodic_run(atimer_t *t)
if (mlsta.mlo_capable == true &&
!hwaddr_is_zero(mlsta.sta[0].bssid))
- mlsta_update_sta_mld(a, ap, &mlsta, s);
+ stamld_update(a, ap, mlsta.macaddr, mlsta.bssid,
+ mlsta.sta[0].mlo_link_id, s);
update_sta_entry(a, ap, &(mlsta.sta[0]));
}
@@ -2350,55 +2351,27 @@ static void wifi_sta_event_handler(void *c, struct blob_attr *msg)
}
#if (EASYMESH_VERSION >= 6)
/* Affiliated STA MAC in-case of MLO client*/
- if (add && data[2] && data[3] && data[4]) {
+ if (ap && add && data[2] && data[3] && data[4]) {
+ uint8_t mlo_bssid[6] = {0};
uint8_t mlo_link_id = 0;
+ struct sta_mld *mld;
mlo_link_id = (uint8_t) blobmsg_get_u32(data[4]);
- if (ap) {
- uint8_t mlo_bssid[6] = {0};
- struct sta_mld *mld;
- int num_sta = 0;
-
- if (!hwaddr_aton(blobmsg_data(data[3]), mlo_macaddr))
- return;
- if (!hwaddr_aton(blobmsg_data(data[2]), mlo_bssid))
- return;
- mld = agent_get_stamld(a, mlo_macaddr);
- if (!mld) {
- struct netif_mld *apmld;
-
- apmld = agent_get_apmld(a, ap->mld_macaddr);
- if(!apmld)
- return;
-
- mld = agent_alloc_stamld(a, apmld, mlo_macaddr);
- if (!mld) {
- err("%s: Failed to allocate MLD:"MACFMT"\n", __func__, MAC2STR(mlo_macaddr));
- return;
- }
- }
-
- memcpy(mld->bssid, mlo_bssid, 6);
-
- /* TODO: code is copy pasted from mlsta_update_sta_mld() */
- num_sta = mld->num_affiliated_sta;
-
- memcpy(s->mld_macaddr, mld->macaddr, 6);
- s->is_affiliated_sta = true;
- s->mlo_link_id = mlo_link_id;
+ if (!hwaddr_aton(blobmsg_data(data[3]), mlo_macaddr))
+ return;
+ if (!hwaddr_aton(blobmsg_data(data[2]), mlo_bssid))
+ return;
- if (!mld_contains_sta(a, mld, macaddr) &&
- num_sta < MAX_AFFILIATED_STA_LINKS_NUM) {
- memcpy(mld->affiliated_sta[num_sta], s->macaddr, 6);
- mld->num_affiliated_sta++;
- }
+ mld = stamld_update(a, ap, mlo_macaddr, mlo_bssid,
+ mlo_link_id, s);
+ if (!mld)
+ return;
- if (s->assoc_frame && s->assoc_frame->len > 4) {
- wifi_process_ml_ie(a, mld,
- s->assoc_frame->frame + 4,
- s->assoc_frame->len - 4);
- }
+ if (s->assoc_frame && s->assoc_frame->len > 4) {
+ wifi_process_ml_ie(a, mld,
+ s->assoc_frame->frame + 4,
+ s->assoc_frame->len - 4);
}
}
#endif
diff --git a/src/mld.c b/src/mld.c
index 19f8d5283732605a3f59323bd5f6c617800bc205..864733c0bb753e63a35bef35837d4bd983a4c03c 100644
--- a/src/mld.c
+++ b/src/mld.c
@@ -8,82 +8,37 @@
#include "backhaul.h"
#include "agent.h"
-void stamld_update(struct agent *a, struct netif_ap *ap,
- struct wifi_mlsta *mlsta, struct sta *s)
+struct sta_mld *stamld_update(struct agent *a, struct netif_ap *ap, uint8_t *mld_macaddr,
+ uint8_t *mld_bssid, uint8_t mlo_link_id, struct sta *s)
{
struct sta_mld *mld;
- int num_sta = 0;
+ int ret;
- mld = agent_get_stamld(a, mlsta->macaddr);
+ mld = agent_get_stamld(a, mld_macaddr);
if (!mld) {
struct netif_mld *apmld;
apmld = agent_get_apmld(a, ap->mld_macaddr);
if (!apmld)
- return;
+ return NULL;
- mld = agent_alloc_stamld(a, apmld, mlsta->macaddr);
+ mld = agent_alloc_stamld(a, apmld, mld_macaddr);
if (!mld)
- return;
- }
-
- memcpy(mld->bssid, mlsta->bssid, 6);
- num_sta = mld->num_affiliated_sta;
-
- memcpy(s->mld_macaddr, mld->macaddr, 6);
- s->is_affiliated_sta = true;
- s->mlo_link_id = mlsta->sta[0].mlo_link_id;
-
- if (!mld_contains_sta(a, mld, mlsta->sta[0].macaddr) &&
- num_sta < MAX_AFFILIATED_STA_LINKS_NUM) {
- memcpy(mld->affiliated_sta[num_sta], s->macaddr, 6);
- mld->num_affiliated_sta++;
+ return NULL;
}
-}
-
-void mlsta_update_sta_mld(struct agent *a, struct netif_ap *ap,
- struct wifi_mlsta *mlsta, struct sta *s)
-{
- struct sta_mld *mld;
- int num_sta = 0;
-
- if (!ap || !mlsta)
- return;
-
- //err("|%s:%d| mldmac:"MACFMT" linkmac:"MACFMT"\n",
- // __func__, __LINE__,
- // MAC2STR(ap->mld->macaddr), MAC2STR(mlsta->macaddr));
-
- mld = agent_get_stamld(a, mlsta->macaddr);
- if (!mld) {
- struct netif_mld *apmld;
-
- apmld = agent_get_apmld(a, ap->mld_macaddr);
- if (!apmld)
- return;
-
- mld = agent_alloc_stamld(a, apmld, mlsta->macaddr);
- if (!mld)
- return;
+ ret = mld_add_sta(a, mld, s->macaddr);
+ if (ret) {
+ dbg("%s: failed to add affiliated STA:"MACFMT" to STAMLD:"MACFMT"\n", __func__, MAC2STR(s->macaddr), MAC2STR(mld_macaddr));
+ return NULL;
}
- memcpy(mld->bssid, mlsta->bssid, 6);
- num_sta = mld->num_affiliated_sta;
+ memcpy(mld->bssid, mld_bssid, 6);
memcpy(s->mld_macaddr, mld->macaddr, 6);
s->is_affiliated_sta = true;
- s->mlo_link_id = mlsta->sta[0].mlo_link_id;
-
- if (!mld_contains_sta(a, mld, mlsta->sta[0].macaddr) &&
- num_sta < MAX_AFFILIATED_STA_LINKS_NUM) {
- memcpy(mld->affiliated_sta[num_sta], s->macaddr, 6);
- mld->num_affiliated_sta++;
- }
-
- //err("|%s:%d| num affiliated_sta:%d\n",
- // __func__, __LINE__,
- // mld->num_affiliated_sta);
+ s->mlo_link_id = mlo_link_id;
+ return mld;
}
/* remove passed affiliated STA from mld */
@@ -422,7 +377,7 @@ void apmld_parse(struct ubus_request *req, int type,
num_ap = mld->num_affiliated_ap;
if (!mld_contains_ap(a, mld, ap->bssid) &&
- num_ap < MAX_AFFILIATED_AP_LINKS_NUM) {
+ num_ap < MAX_AFFILIATED_AP_LINKS_NUM) {
memcpy(mld->affiliated_ap[num_ap], ap->bssid, 6);
mld->num_affiliated_ap++;
} else {
@@ -501,7 +456,7 @@ void mld_parse_affiliated_bsta(struct agent *a, struct bsta_mld *mld,
bk->mlo_link_id = link_id;
if (!mld_contains_bsta(a, mld, bk->macaddr) &&
- num_sta < MAX_AFFILIATED_STA_LINKS_NUM) {
+ num_sta < MAX_AFFILIATED_STA_LINKS_NUM) {
memcpy(mld->affiliated_bsta[num_sta], bk->macaddr, 6);
mld->num_affiliated_bsta++;
}
@@ -647,6 +602,56 @@ struct netif_mld *agent_alloc_apmld(struct agent *a, const char *ifname)
return mld;
}
+int mld_add_ap(struct agent *a, struct netif_mld *mld,
+ uint8_t *macaddr)
+{
+ int num_ap = mld->num_affiliated_ap;
+
+ if (mld_contains_ap(a, mld, macaddr))
+ return 0;
+
+ if (num_ap >= MAX_AFFILIATED_AP_LINKS_NUM)
+ return -1;
+
+ memcpy(mld->affiliated_ap[num_ap], macaddr, 6);
+ mld->num_affiliated_ap++;
+ return 0;
+}
+
+int mld_add_sta(struct agent *a, struct sta_mld *mld,
+ uint8_t *macaddr)
+{
+ int num_sta = mld->num_affiliated_sta;
+
+ if (mld_contains_sta(a, mld, macaddr))
+ return 0;
+
+ if (num_sta >= MAX_AFFILIATED_STA_LINKS_NUM)
+ return -1;
+
+ memcpy(mld->affiliated_sta[num_sta], macaddr, 6);
+ mld->num_affiliated_sta++;
+ return 0;
+}
+
+
+int mld_add_bsta(struct agent *a, struct bsta_mld *mld,
+ uint8_t *macaddr)
+{
+ int num_bsta = mld->num_affiliated_bsta;
+
+ if (mld_contains_bsta(a, mld, macaddr))
+ return 0;
+
+ if (num_bsta >= MAX_AFFILIATED_BSTA_LINKS_NUM)
+ return -1;
+
+ memcpy(mld->affiliated_bsta[num_bsta], macaddr, 6);
+ mld->num_affiliated_bsta++;
+ return 0;
+}
+
+
bool mld_contains_ap(struct agent *a, struct netif_mld *mld,
uint8_t *macaddr)
diff --git a/src/mld.h b/src/mld.h
index ecec4f49337d00c957abd644a470c649ea163059..da04fb78652b7dd8f3f8647cdcd068f363fd2780 100644
--- a/src/mld.h
+++ b/src/mld.h
@@ -65,9 +65,15 @@ struct sta_mld {
};
-
-void mlsta_update_sta_mld(struct agent *a, struct netif_ap *ap,
- struct wifi_mlsta *mlsta, struct sta *s);
+struct sta_mld *stamld_update(struct agent *a, struct netif_ap *ap,
+ uint8_t *mld_macaddr, uint8_t *mld_bssid,
+ uint8_t mlo_link_id, struct sta *s);
+int mld_add_ap(struct agent *a, struct netif_mld *mld,
+ uint8_t *macaddr);
+int mld_add_sta(struct agent *a, struct sta_mld *mld,
+ uint8_t *macaddr);
+int mld_add_bsta(struct agent *a, struct bsta_mld *mld,
+ uint8_t *macaddr);
int mld_del_sta(struct agent *a, struct sta *s);
int mld_del_bsta(struct agent *a, struct netif_bk *bk);
int mld_del_ap(struct agent *a, struct netif_ap *ap);