diff --git a/src/Makefile b/src/Makefile index d660f5e4de86a0ec7aa1cceb599d38e643f00464..c8ee73068e0a7b3c296703c173cc3180610a36f5 100644 --- a/src/Makefile +++ b/src/Makefile @@ -38,6 +38,10 @@ OBJS = \ OBJS += steer_module.o +ifneq (,$(findstring EASYMESH_VENDOR_EXT,$(CFLAGS))) +OBJS += cntlr_extension.o +endif + LIBS = -lubus -lubox -ljson-c -lblobmsg_json -luci -pthread LIBS += -rdynamic -ldl LIBS += -leasy -lwifiutils diff --git a/src/cntlr.h b/src/cntlr.h index 58139fdc2176b41903c19196d4f05bbfafaa2a7b..0fb959de6a3b13ac11b4184cecba4ca2facad5ab 100644 --- a/src/cntlr.h +++ b/src/cntlr.h @@ -34,10 +34,6 @@ struct tlv_rx_linkmetric; struct tlv_tx_linkmetric; -#ifndef EASYMESH_VENDOR_EXT_OUI_DEFAULT -#define EASYMESH_VENDOR_EXT_OUI_DEFAULT 0xB456FA /* IOPSYS OUI */ -#endif - #ifndef LIMIT_STA_COUNT #define LIMIT_STA_COUNT (100) #endif diff --git a/src/cntlr_cmdu.c b/src/cntlr_cmdu.c index 083cb0b84f58d2970afc8e406db5df4b8621e2a4..0f8a61ca68a0aded3ac055e4059b6e6add3b8e47 100644 --- a/src/cntlr_cmdu.c +++ b/src/cntlr_cmdu.c @@ -1462,28 +1462,3 @@ int send_agent_list_to_all_nodes(struct controller *c) #endif /* EASYMESH_VERSION > 2 */ -#ifdef EASYMESH_VENDOR_EXT -struct cmdu_buff *cntlr_gen_vendor_specific_disassociate_sta(struct controller *c, - uint8_t *sta, uint16_t reason) -{ - struct cmdu_buff *frm; - int ret; - uint16_t mid = 0; - - frm = cmdu_alloc_simple(CMDU_TYPE_VENDOR_SPECIFIC, &mid); - if (!frm) { - dbg("%s: -ENOMEM\n", __func__); - return NULL; - } - - ret = cntlr_gen_vendor_specific_disassociate_sta_tlv(c, frm, sta, reason); - if (ret) - goto out; - - cmdu_put_eom(frm); - return frm; -out: - cmdu_free(frm); - return NULL; -} -#endif diff --git a/src/cntlr_cmdu.h b/src/cntlr_cmdu.h index 8ea5b7ea60ca480975f83dfa6742fe5dcb03a12b..fbbf88dcaa1a861dc09ee7cce2de24cccba6f646 100644 --- a/src/cntlr_cmdu.h +++ b/src/cntlr_cmdu.h @@ -144,8 +144,4 @@ struct cmdu_buff *cntlr_gen_agent_list(struct controller *c); int send_agent_list_to_all_nodes(struct controller *c); #endif /* EASYMESH_VERSION > 2 */ -#ifdef EASYMESH_VENDOR_EXT -struct cmdu_buff *cntlr_gen_vendor_specific_disassociate_sta(struct controller *c, - uint8_t *sta, uint16_t reason); -#endif #endif /* CNTLR_CMDU_GEN_H */ diff --git a/src/cntlr_commands_impl.c b/src/cntlr_commands_impl.c index a9a7156f66d61b5111051e01932706b8c02287f2..be5607927411a6fdd7404e8d3ff5c51d2bb2b431 100644 --- a/src/cntlr_commands_impl.c +++ b/src/cntlr_commands_impl.c @@ -31,6 +31,7 @@ #include "sta.h" #include "wifi_opclass.h" #include "wifi_dataelements.h" +#include "cntlr_extension.h" static const char *blobmsg_type2str(unsigned int type) { diff --git a/src/cntlr_extension.c b/src/cntlr_extension.c new file mode 100644 index 0000000000000000000000000000000000000000..1f3304e5c3d45b385362334418a20353823f05ff --- /dev/null +++ b/src/cntlr_extension.c @@ -0,0 +1,159 @@ +#ifdef EASYMESH_VENDOR_EXT + +#include <stdio.h> +#include <string.h> +#include <cmdu.h> +#include "cntlr.h" +#include "cntlr_tlv.h" +#include "cntlr_ubus.h" +#include "cntlr_extension.h" +#include "utils/debug.h" + + +#ifdef PROPAGATE_PROBE_REQ + +int handle_vendor_specific_probe_req(void *cntlr, struct cmdu_buff *rx_cmdu, + struct tlv_vendor_probe_req *tlv) +{ + struct controller *c = (struct controller *) cntlr; + char evt_data[120]; + + /* process probe request from agent */ + snprintf(evt_data, sizeof(evt_data), + "{\"macaddr\":\"" MACFMT "\", " + "\"almacaddr\":\"" MACFMT "\", " + "\"bssid\":\"" MACFMT "\", " + "\"rcpi\":%" PRIu8 "}", + MAC2STR(tlv->macaddr), + MAC2STR(rx_cmdu->origin), + MAC2STR(tlv->bssid), + tlv->rcpi); + cntlr_notify_event(c, "probe-req", evt_data); + + return 0; +} + +#endif + +int handle_vendor_extension(void *cntlr, struct cmdu_buff *rx_cmdu, struct node *n) +{ + struct tlv *tv[1][TLV_MAXNUM] = {0}; + struct tlv_vendor_specific *tlv; + struct tlv_vendor_impl *tlv_impl; + const uint8_t vendor_oui[4] = {0}; + uint32_t oui = 0; + + if (!map_cmdu_validate_parse(rx_cmdu, tv, ARRAY_SIZE(tv), n->map_profile)) { + err("cmdu validation: [VENDOR_SPECIFIC] failed\n"); + return -1; + } + + if (!tv[0][0]) { + return -1; + } + + tlv = (struct tlv_vendor_specific *)tv[0][0]->data; + + BUF_PUT_BE24(vendor_oui, EASYMESH_VENDOR_EXT_OUI_DEFAULT); +#ifdef EASYMESH_VENDOR_EXT_OUI + oui = EASYMESH_VENDOR_EXT_OUI; + BUF_PUT_BE24(vendor_oui, oui); +#endif + + if (memcmp(vendor_oui, tlv->oui, 3)) { + warn("%s: unknown vendor TLV\n", __func__); + return -1; + } + + if (tv[0][0]->len < sizeof(struct tlv_vendor_impl)) { + warn("%s: invalid length of vendor TLV\n", __func__); + return -1; + } + + tlv_impl = (struct tlv_vendor_impl *)tv[0][0]->data; + + switch (tlv_impl->vendor_type) { +#ifdef PROPAGATE_PROBE_REQ + case TLV_VENDOR_TYPE_IMPL_PROBE_REQ: + if (tv[0][0]->len < sizeof(struct tlv_vendor_probe_req)) { + warn("%s: invalid length of vendor Probe Request TLV\n", + __func__); + return -1; + } + return handle_vendor_specific_probe_req(cntlr, rx_cmdu, + (struct tlv_vendor_probe_req *)tlv_impl); +#endif + default: + warn("%s: unknown type of vendor TLV\n", __func__); + break; + } + + return -1; +} + +int cntlr_gen_vendor_specific_disassociate_sta_tlv(struct controller *c, + struct cmdu_buff *frm, uint8_t *sta, uint16_t reason) +{ + struct tlv *t; + struct tlv_vendor_disassociate_sta *data; + int ret; + const uint8_t vendor_oui[4] = {0}; + uint32_t oui = 0; + + BUF_PUT_BE24(vendor_oui, EASYMESH_VENDOR_EXT_OUI_DEFAULT); +#ifdef EASYMESH_VENDOR_EXT_OUI + oui = EASYMESH_VENDOR_EXT_OUI; + BUF_PUT_BE24(vendor_oui, oui); +#endif + + /* prepare TLVs */ + t = cmdu_reserve_tlv(frm, 32); + if (!t) { + dbg("%s: -ENOMEM\n", __func__); + return -1; + } + + t->type = TLV_TYPE_VENDOR_SPECIFIC; + t->len = sizeof(struct tlv_vendor_disassociate_sta); + data = (struct tlv_vendor_disassociate_sta *)t->data; + + memcpy(data->vendor_impl.vendor.oui, vendor_oui, 3); + data->vendor_impl.vendor_type = TLV_VENDOR_TYPE_IMPL_DISASSOCIATE_STA; + + memcpy(data->macaddr, sta, 6); + BUF_PUT_BE16(data->reason, reason); + + ret = cmdu_put_tlv(frm, t); + if (ret) { + err("%s: error: cmdu_put_tlv()\n", __func__); + return -1; + } + + return 0; +} + +struct cmdu_buff *cntlr_gen_vendor_specific_disassociate_sta(struct controller *c, + uint8_t *sta, uint16_t reason) +{ + struct cmdu_buff *frm; + int ret; + uint16_t mid = 0; + + frm = cmdu_alloc_simple(CMDU_TYPE_VENDOR_SPECIFIC, &mid); + if (!frm) { + dbg("%s: -ENOMEM\n", __func__); + return NULL; + } + + ret = cntlr_gen_vendor_specific_disassociate_sta_tlv(c, frm, sta, reason); + if (ret) + goto out; + + cmdu_put_eom(frm); + return frm; +out: + cmdu_free(frm); + return NULL; +} + +#endif diff --git a/src/cntlr_extension.h b/src/cntlr_extension.h new file mode 100644 index 0000000000000000000000000000000000000000..17d8de48e3a9891667a8675457ccad3b31f2e1a1 --- /dev/null +++ b/src/cntlr_extension.h @@ -0,0 +1,49 @@ +#ifndef CNTLR_EXTENSION_H +#define CNTLR_EXTENSION_H + +#ifdef EASYMESH_VENDOR_EXT +#include <1905_tlvs.h> + +#ifndef EASYMESH_VENDOR_EXT_OUI_DEFAULT +#define EASYMESH_VENDOR_EXT_OUI_DEFAULT 0xB456FA /* IOPSYS OUI */ +#endif + +/* Vendor TLV types supported by our implementation */ +enum tlv_vendor_type_impl { + TLV_VENDOR_TYPE_IMPL_UNSPEC = 0, + TLV_VENDOR_TYPE_IMPL_PROBE_REQ = 1, /* Probe Requests */ + TLV_VENDOR_TYPE_IMPL_DISASSOCIATE_STA = 2, /* Disassociate STA */ +}; + +/* Vendor TLV structure for our implementation */ +struct tlv_vendor_impl { + struct tlv_vendor_specific vendor; + uint8_t vendor_type; /* Belongs to tlv_vendor_type_impl enumeration */ +} __attribute__((packed)); + +struct tlv_vendor_disassociate_sta { + struct tlv_vendor_impl vendor_impl; + uint8_t macaddr[6]; + uint16_t reason; +} __attribute__((packed)); + +struct tlv_vendor_probe_req { + struct tlv_vendor_impl vendor_impl; + uint8_t macaddr[6]; + uint8_t bssid[6]; + uint8_t rcpi; +} __attribute__((packed)); + +#ifdef PROPAGATE_PROBE_REQ +int handle_vendor_specific_probe_req(void *cntlr, struct cmdu_buff *rx_cmdu, + struct tlv_vendor_probe_req *tlv); +#endif + +int handle_vendor_extension(void *cntlr, struct cmdu_buff *rx_cmdu, struct node *n); +int cntlr_gen_vendor_specific_disassociate_sta_tlv(struct controller *c, + struct cmdu_buff *frm, uint8_t *sta, uint16_t reason); +struct cmdu_buff *cntlr_gen_vendor_specific_disassociate_sta(struct controller *c, + uint8_t *sta, uint16_t reason); +#endif + +#endif /* CNTLR_EXTENSION_H */ diff --git a/src/cntlr_map.c b/src/cntlr_map.c index 545ad403f7e526f827ea72e1280be8d6ff9fac82..11c867f68c997f18f48f84d2d277326fa8f2a7f6 100644 --- a/src/cntlr_map.c +++ b/src/cntlr_map.c @@ -55,6 +55,7 @@ #include "topology.h" #include "wifi_dataelements.h" #include "wifi_opclass.h" +#include "cntlr_extension.h" /* Length of data in CMDU after which we consider that CMDU might might be fragmentized */ #define FRAG_DATA_SIZE (1400) @@ -659,120 +660,12 @@ int handle_topology_response(void *cntlr, struct cmdu_buff *cmdu, struct node *n return 0; } -#ifdef EASYMESH_VENDOR_EXT -#ifdef PROPAGATE_PROBE_REQ - -int handle_vendor_specific_probe_req(void *cntlr, struct cmdu_buff *rx_cmdu, - struct tlv_vendor_probe_req *tlv) -{ - struct controller *c = (struct controller *) cntlr; - char evt_data[120]; - - /* process probe request from agent */ - /* { "map.controller": {"event":"<ev_type>","data":{<ev_data>}} } */ - snprintf(evt_data, sizeof(evt_data), - "{\"macaddr\":\"" MACFMT "\", " - "\"almacaddr\":\"" MACFMT "\", " - "\"bssid\":\"" MACFMT "\", " - "\"rcpi\":%" PRIu8 "}", - MAC2STR(tlv->macaddr), - MAC2STR(rx_cmdu->origin), - MAC2STR(tlv->bssid), - tlv->rcpi); - cntlr_notify_event(c, "probe-req", evt_data); - - return 0; -} - -#endif -#endif - - -#ifdef EASYMESH_VENDOR_EXT -static bool validate_vendor_specific(struct cmdu_buff *cmdu, struct tlv *tlvs[][TLV_MAXNUM], size_t tlvs_size, uint8_t profile) -{ - struct tlv_policy a_policy[] = { - [0] = { .type = TLV_TYPE_VENDOR_SPECIFIC, - .present = TLV_PRESENT_OPTIONAL_MORE, - .minlen = 3, - }, - }; - int ret; - - trace("%s |" MACFMT "|CMDU: vendor specific\n", - __func__, MAC2STR(cmdu->origin)); - - ret = cmdu_parse_tlvs(cmdu, tlvs, a_policy, 1); - if (ret) { - dbg("%s: parse_tlv failed\n", __func__); - return false; - } - - if (!tlvs[0][0]) { - dbg("%s: Missing one or more mandatory TLV!\n", __func__); - return false; - } - - return true; -} -#endif - int handle_vendor_specific(void *cntlr, struct cmdu_buff *rx_cmdu, struct node *n) { trace("%s: --->\n", __func__); #ifdef EASYMESH_VENDOR_EXT - struct tlv *tv[1][TLV_MAXNUM] = {0}; - struct tlv_vendor_specific *tlv; - struct tlv_vendor_impl *tlv_impl; - const uint8_t vendor_oui[4] = {0}; - uint32_t oui = 0; - - if (!validate_vendor_specific(rx_cmdu, tv, 1, n->map_profile)) { - err("cmdu validation: [VENDOR_SPECIFIC] failed\n"); - return -1; - } - - if (!tv[0][0]) { - return -1; - } - - tlv = (struct tlv_vendor_specific *)tv[0][0]->data; - - BUF_PUT_BE24(vendor_oui, EASYMESH_VENDOR_EXT_OUI_DEFAULT); -#ifdef EASYMESH_VENDOR_EXT_OUI - oui = EASYMESH_VENDOR_EXT_OUI; - BUF_PUT_BE24(vendor_oui, oui); -#endif - - if (memcmp(vendor_oui, tlv->oui, 3)) { - warn("%s: unknown vendor TLV\n", __func__); - return -1; - } - - if (tv[0][0]->len < sizeof(struct tlv_vendor_impl)) { - warn("%s: invalid length of vendor TLV\n", __func__); - return -1; - } - - tlv_impl = (struct tlv_vendor_impl *)tv[0][0]->data; - - switch (tlv_impl->vendor_type) { -#ifdef PROPAGATE_PROBE_REQ - case TLV_VENDOR_TYPE_IMPL_PROBE_REQ: - if (tv[0][0]->len < sizeof(struct tlv_vendor_probe_req)) { - warn("%s: invalid length of vendor Probe Request TLV\n", - __func__); - return -1; - } - return handle_vendor_specific_probe_req(cntlr, - rx_cmdu, (struct tlv_vendor_probe_req *)tlv_impl); -#endif - default: - warn("%s: unknown type of vendor TLV\n", __func__); - break; - } - + return handle_vendor_extension(cntlr, rx_cmdu, n); #endif return -1; } diff --git a/src/cntlr_tlv.c b/src/cntlr_tlv.c index 1f0ec6c4ee4384059cdfdfe0ede09ef4b9507583..eca7759fab2e53e26d3192b592ddf52e9f18921e 100644 --- a/src/cntlr_tlv.c +++ b/src/cntlr_tlv.c @@ -2796,46 +2796,3 @@ int cntlr_gen_eht_operations(struct controller *c, struct cmdu_buff *frm, return ret; } #endif - -#ifdef EASYMESH_VENDOR_EXT -int cntlr_gen_vendor_specific_disassociate_sta_tlv(struct controller *c, - struct cmdu_buff *frm, uint8_t *sta, uint16_t reason) -{ - struct tlv *t; - struct tlv_vendor_disassociate_sta *data; - int ret; - const uint8_t vendor_oui[4] = {0}; - uint32_t oui = 0; - - BUF_PUT_BE24(vendor_oui, EASYMESH_VENDOR_EXT_OUI_DEFAULT); -#ifdef EASYMESH_VENDOR_EXT_OUI - oui = EASYMESH_VENDOR_EXT_OUI; - BUF_PUT_BE24(vendor_oui, oui); -#endif - - /* prepare TLVs */ - t = cmdu_reserve_tlv(frm, 32); - if (!t) { - dbg("%s: -ENOMEM\n", __func__); - return -1; - } - - t->type = TLV_TYPE_VENDOR_SPECIFIC; - t->len = sizeof(struct tlv_vendor_disassociate_sta); - data = (struct tlv_vendor_disassociate_sta *)t->data; - - memcpy(data->vendor_impl.vendor.oui, vendor_oui, 3); - data->vendor_impl.vendor_type = TLV_VENDOR_TYPE_IMPL_DISASSOCIATE_STA; - - memcpy(data->macaddr, sta, 6); - BUF_PUT_BE16(data->reason, reason); - - ret = cmdu_put_tlv(frm, t); - if (ret) { - err("%s: error: cmdu_put_tlv()\n", __func__); - return -1; - } - - return 0; -} -#endif diff --git a/src/cntlr_tlv.h b/src/cntlr_tlv.h index bb64f3dd65a78a275111814ceb8a4bdecb19eb8d..68acf3af28db97f8692c3132a84a726ffc75d253 100644 --- a/src/cntlr_tlv.h +++ b/src/cntlr_tlv.h @@ -31,36 +31,6 @@ struct wifi7_radio_capabilities; struct wifi_radio_opclass; struct wps_credential; -#ifdef EASYMESH_VENDOR_EXT - -/* Vendor TLV types supported by our implementation */ -enum tlv_vendor_type_impl { - TLV_VENDOR_TYPE_IMPL_UNSPEC = 0, - TLV_VENDOR_TYPE_IMPL_PROBE_REQ = 1, /* Probe Requests */ - TLV_VENDOR_TYPE_IMPL_DISASSOCIATE_STA = 2, /* Disassociate STA */ -}; - -/* Vendor TLV structure for our implementation */ -struct tlv_vendor_impl { - struct tlv_vendor_specific vendor; - uint8_t vendor_type; /* Belongs to tlv_vendor_type_impl enumeration */ -} __attribute__((packed)); - -struct tlv_vendor_disassociate_sta { - struct tlv_vendor_impl vendor_impl; - uint8_t macaddr[6]; - uint16_t reason; -} __attribute__((packed)); - -struct tlv_vendor_probe_req { - struct tlv_vendor_impl vendor_impl; - uint8_t macaddr[6]; - uint8_t bssid[6]; - uint8_t rcpi; -} __attribute__((packed)); - -#endif - uint8_t *extract_tlv_by_type(struct cmdu_buff *cmdu, uint8_t tlv_type); int cntlr_gen_8021q_settings(struct controller *c, struct cmdu_buff *frm); int cntlr_gen_traffic_sep_policy(struct controller *c, struct cmdu_buff *frm); @@ -231,8 +201,4 @@ struct cmdu_buff *cntlr_gen_channel_sel_request(struct controller *c, uint8_t *radio_id, struct wifi_radio_opclass *opclass); -#ifdef EASYMESH_VENDOR_EXT -int cntlr_gen_vendor_specific_disassociate_sta_tlv(struct controller *c, - struct cmdu_buff *frm, uint8_t *sta, uint16_t reason); -#endif #endif /* CNTLR_TLV_GEN_H */ diff --git a/src/config.c b/src/config.c index b1695a83a298d1240c05d1ecd1b53a8b34dceb6a..930ec4d883be6a1c93ec1d703410995d33df8d4e 100644 --- a/src/config.c +++ b/src/config.c @@ -34,6 +34,7 @@ #include "steer_module.h" #include "utils/debug.h" #include "utils/utils.h" +#include "cntlr_extension.h" static int clean_agentlist(struct node_policy *p)