diff --git a/src/Makefile b/src/Makefile index 53f2353be905112fd6269feee08f8f08002f2b00..86b626e99f7df0678da0f39e21fc883b07fbc1b5 100644 --- a/src/Makefile +++ b/src/Makefile @@ -53,6 +53,10 @@ ifneq (,$(findstring VENDOR_EXTENSION,$(CFLAGS))) AGENT_OBJS += extension.o endif +ifneq (,$(findstring EASYMESH_VENDOR_EXT,$(CFLAGS))) +AGENT_OBJS += vendor.o +endif + LIBS = -lubus -lubox -ljson-c -lblobmsg_json -luci -leasy LIBS += -I/usr/include/libnl3 -lnl-3 -lnl-route-3 LIBS += -rdynamic -ldl diff --git a/src/agent.c b/src/agent.c index 10e0506ddb6a4437d218792b821a0ec9969e4134..2f33ed5e279598859b24cf8d7fb1c425007e7a9b 100644 --- a/src/agent.c +++ b/src/agent.c @@ -69,6 +69,7 @@ #include "utils/utils.h" #include "wifi.h" #include "wifi_caps.h" +#include "vendor.h" struct json_object; diff --git a/src/agent.h b/src/agent.h index b8814114646c6179b644405e02c7f228a58d7f4b..d5ff7725d8fd7717dc8707311a9b064167b6b8de 100644 --- a/src/agent.h +++ b/src/agent.h @@ -34,10 +34,6 @@ struct blob_attr; -#ifndef EASYMESH_VENDOR_EXT_OUI_DEFAULT -#define EASYMESH_VENDOR_EXT_OUI_DEFAULT 0xB456FA /* IOPSYS OUI */ -#endif - #ifdef VENDOR_EXTENSION struct registered_plugin { void *plugin; diff --git a/src/agent_cmdu.c b/src/agent_cmdu.c index 607bbc1fa487a059a1af06b610b6be92641b3235..4d4d580be62145ed35c2e92d4f86dd7766e4168b 100644 --- a/src/agent_cmdu.c +++ b/src/agent_cmdu.c @@ -2284,37 +2284,3 @@ out: } #endif // EASYMESH_VERSION >= 6 -#ifdef EASYMESH_VENDOR_EXT - -struct cmdu_buff *agent_gen_vendor_specific_probe_req( - struct agent *a, - uint8_t *sta_mac, - uint8_t *bssid, - uint8_t rcpi) -{ - uint16_t mid = 0; - int ret; - struct cmdu_buff *frm; - - frm = cmdu_alloc_simple(CMDU_TYPE_VENDOR_SPECIFIC, &mid); - if (!frm) { - dbg("%s: -ENOMEM\n", __func__); - return NULL; - } - - memcpy(frm->origin, a->cntlr_almac, 6); - - ret = agent_gen_vendor_specific_probe_req_tlv(a, frm, sta_mac, bssid, - rcpi); - if (ret != 0) - goto error; - - cmdu_put_eom(frm); - return frm; - -error: - cmdu_free(frm); - return NULL; -} - -#endif diff --git a/src/agent_cmdu.h b/src/agent_cmdu.h index 1e9b4b34674290793153c24ddc30a1861b205050..fd100dc763bacceca911a29862d190fa9d79b2cc 100644 --- a/src/agent_cmdu.h +++ b/src/agent_cmdu.h @@ -100,14 +100,6 @@ struct cmdu_buff *agent_gen_topology_discovery(struct agent *a); struct cmdu_buff *agent_gen_failed_connection(struct agent *a, uint8_t *sta, int status_code, int reason_code); -#ifdef EASYMESH_VENDOR_EXT -struct cmdu_buff *agent_gen_vendor_specific_probe_req( - struct agent *a, - uint8_t *sta_mac, - uint8_t *bssid, - uint8_t rcpi); -#endif - #if (EASYMESH_VERSION > 2) struct cmdu_buff *agent_gen_bss_configuration_request(struct agent *a); struct cmdu_buff *agent_gen_bss_configuration_result(struct agent *a); diff --git a/src/agent_map.c b/src/agent_map.c index 6b7716d60e7d6b5bcca6832fd8af405ccfbd46b0..e3bbf5953e0affb0e679512c29879aaf949447d0 100644 --- a/src/agent_map.c +++ b/src/agent_map.c @@ -69,6 +69,7 @@ #include "wifi_opclass.h" #include "wifi_scanresults.h" #include "wifi_ubus.h" +#include "vendor.h" struct timespec; @@ -1171,87 +1172,12 @@ int handle_topology_response(void *agent, struct cmdu_buff *cmdu, return 0; } -#ifdef EASYMESH_VENDOR_EXT -int handle_vendor_specific_disassociate_sta(void *agent, - struct tlv_vendor_disassociate_sta *tlv) -{ - struct agent *a = (struct agent *) agent; - struct netif_ap *ap; - uint16_t reason = 0; - struct sta *s; - - s = agent_get_sta(a, tlv->macaddr); - if (!s) { - warn("%s: STA ("MACFMT") not found\n", __func__, - MAC2STR(tlv->macaddr)); - return -1; - } - - ap = agent_get_ap(a, s->bssid); - if (!ap) { - warn("%s: AP ("MACFMT") not found\n", __func__, - MAC2STR(s->bssid)); - return -1; - } - - reason = BUF_GET_BE16(tlv->reason); - wifi_ubus_disconnect_sta(a->ubus_ctx, ap->ifname, - tlv->macaddr, reason); - return 0; -} -#endif - int handle_vendor_specific(void *agent, 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 (!map_cmdu_validate_parse(rx_cmdu, tv, ARRAY_SIZE(tv), n->map_profile)) { - dbg("%s: Invalid Vendor Specific CMDU, err = (%d) '%s'\n", __func__, - map_error, map_strerror(map_error)); - 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) { - case TLV_VENDOR_TYPE_IMPL_DISASSOCIATE_STA: - if (tv[0][0]->len < sizeof(struct tlv_vendor_disassociate_sta)) { - warn("%s: invalid length of vendor Disassociate STA TLV\n", - __func__); - return -1; - } - return handle_vendor_specific_disassociate_sta(agent, - (struct tlv_vendor_disassociate_sta *)tlv_impl); - default: - warn("%s: unknown type of vendor TLV\n", __func__); - break; - } - + return handle_vendor_extension(agent, rx_cmdu, n); #endif return -1; } diff --git a/src/agent_tlv.c b/src/agent_tlv.c index baab366094dd9b2999e275604213d34d296d80dc..3dda667d2f95233d7f4a8bb3bbc5f4559525635c 100644 --- a/src/agent_tlv.c +++ b/src/agent_tlv.c @@ -2998,53 +2998,6 @@ struct tlv_vendor_specific *agent_gen_vendor_specific_tlv(struct agent *a, uint8 return NULL; } -#ifdef EASYMESH_VENDOR_EXT -int agent_gen_vendor_specific_probe_req_tlv(struct agent *a, - struct cmdu_buff *frm, uint8_t *sta_mac, uint8_t *bssid, - uint8_t rcpi) -{ - struct tlv *t; - struct tlv_vendor_probe_req *data; - int ret; - const uint8_t vendor_oui[4] = {0}; -#ifdef EASYMESH_VENDOR_EXT_OUI - uint32_t oui = 0; -#endif - - 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, 40); - if (!t) { - dbg("%s: -ENOMEM\n", __func__); - return -1; - } - - t->type = TLV_TYPE_VENDOR_SPECIFIC; - t->len = sizeof(struct tlv_vendor_probe_req); - data = (struct tlv_vendor_probe_req *)t->data; - - memcpy(data->vendor_impl.vendor.oui, vendor_oui, 3); - data->vendor_impl.vendor_type = TLV_VENDOR_TYPE_IMPL_PROBE_REQ; - memcpy(data->macaddr, sta_mac, 6); - memcpy(data->bssid, bssid, 6); - data->rcpi = rcpi; - - ret = cmdu_put_tlv(frm, t); - if (ret) { - err("%s: error: cmdu_put_tlv()\n", __func__); - return -1; - } - - return 0; -} - -#endif /*EASYMESH_VENDOR_EXT*/ - char *get_timestamp_old(time_t *t, char **tbuf) { char tmpbuf[64] = {0}; diff --git a/src/agent_tlv.h b/src/agent_tlv.h index 5ca39f6bdbc719562b974d3f63a6482e46df9955..200cb8c9edb4b703dff4151e7e96e185d231459e 100644 --- a/src/agent_tlv.h +++ b/src/agent_tlv.h @@ -41,36 +41,6 @@ struct wifi_scanres_channel_element; #define BUF_PUT_BE64(b, v) buf_put_be64((uint8_t *)&(b), v) -#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 - int get_radio_index(struct agent *a, uint8_t *mac); int get_bss_index(struct wifi_radio_element *radio, uint8_t *bssid); int get_radio_and_bss_index(struct agent *a, uint8_t *bssid, int *radio_index); @@ -161,10 +131,7 @@ int agent_gen_tunnel_msg_type(struct agent *a, int agent_gen_tunneled(struct agent *a, struct cmdu_buff *frm, int frame_len, uint8_t *frame_body); struct tlv_vendor_specific *agent_gen_vendor_specific_tlv(struct agent *a, uint8_t depth); -#ifdef EASYMESH_VENDOR_EXT -int agent_gen_vendor_specific_probe_req_tlv(struct agent *a, - struct cmdu_buff *frm, uint8_t *sta_mac, uint8_t *bssid, uint8_t rcpi); -#endif + int agent_gen_timestamp_tlv(struct agent *agent, struct cmdu_buff *frm); int agent_gen_ch_scan_response_tlv(struct agent *a, struct cmdu_buff *cmdu, uint8_t *radio_mac, uint8_t opclass_id, @@ -261,9 +228,4 @@ int agent_gen_associated_sta_mld_config(struct agent *a, struct cmdu_buff *frm, struct sta_mld *s); #endif -#ifdef EASYMESH_VENDOR_EXT -int agent_gen_vendor_specific_probe_req_tlv(struct agent *a, - struct cmdu_buff *frm, uint8_t *sta_mac, uint8_t *bssid, uint8_t rcpi); -#endif - #endif /* MAPAGENT_TLV_H */ diff --git a/src/vendor.c b/src/vendor.c new file mode 100644 index 0000000000000000000000000000000000000000..05bb4683568def258273bc36238a1cc50f0e0f67 --- /dev/null +++ b/src/vendor.c @@ -0,0 +1,163 @@ +#ifdef EASYMESH_VENDOR_EXT + +#include <stdio.h> +#include <string.h> +#include <cmdu.h> +#include "agent.h" +#include "utils/debug.h" +#include "wifi_ubus.h" +#include "vendor.h" + + +int agent_gen_vendor_specific_probe_req_tlv(struct agent *a, + struct cmdu_buff *frm, uint8_t *sta_mac, uint8_t *bssid, + uint8_t rcpi) +{ + struct tlv *t; + struct tlv_vendor_probe_req *data; + int ret; + const uint8_t vendor_oui[4] = {0}; +#ifdef EASYMESH_VENDOR_EXT_OUI + uint32_t oui = 0; +#endif + + 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, 40); + if (!t) { + dbg("%s: -ENOMEM\n", __func__); + return -1; + } + + t->type = TLV_TYPE_VENDOR_SPECIFIC; + t->len = sizeof(struct tlv_vendor_probe_req); + data = (struct tlv_vendor_probe_req *)t->data; + + memcpy(data->vendor_impl.vendor.oui, vendor_oui, 3); + data->vendor_impl.vendor_type = TLV_VENDOR_TYPE_IMPL_PROBE_REQ; + memcpy(data->macaddr, sta_mac, 6); + memcpy(data->bssid, bssid, 6); + data->rcpi = rcpi; + ret = cmdu_put_tlv(frm, t); + if (ret) { + err("%s: error: cmdu_put_tlv()\n", __func__); + return -1; + } + + return 0; +} + +struct cmdu_buff *agent_gen_vendor_specific_probe_req( + struct agent *a, + uint8_t *sta_mac, + uint8_t *bssid, + uint8_t rcpi) +{ + uint16_t mid = 0; + int ret; + struct cmdu_buff *frm; + + frm = cmdu_alloc_simple(CMDU_TYPE_VENDOR_SPECIFIC, &mid); + if (!frm) { + dbg("%s: -ENOMEM\n", __func__); + return NULL; + } + + memcpy(frm->origin, a->cntlr_almac, 6); + + ret = agent_gen_vendor_specific_probe_req_tlv(a, frm, sta_mac, bssid, + rcpi); + if (ret != 0) + goto error; + + cmdu_put_eom(frm); + return frm; + +error: + cmdu_free(frm); + return NULL; +} + +int handle_vendor_specific_disassociate_sta(void *agent, + struct tlv_vendor_disassociate_sta *tlv) +{ + struct agent *a = (struct agent *) agent; + struct netif_ap *ap; + uint16_t reason = 0; + struct sta *s; + + s = agent_get_sta(a, tlv->macaddr); + if (!s) { + warn("%s: STA ("MACFMT") not found\n", __func__, + MAC2STR(tlv->macaddr)); + return -1; + } + + ap = agent_get_ap(a, s->bssid); + if (!ap) { + warn("%s: AP ("MACFMT") not found\n", __func__, + MAC2STR(s->bssid)); + return -1; + } + + reason = BUF_GET_BE16(tlv->reason); + wifi_ubus_disconnect_sta(a->ubus_ctx, ap->ifname, + tlv->macaddr, reason); + return 0; +} + +int handle_vendor_extension(void *agent, 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)) { + dbg("%s: Invalid Vendor Specific CMDU, err = (%d) '%s'\n", __func__, + map_error, map_strerror(map_error)); + 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) { + case TLV_VENDOR_TYPE_IMPL_DISASSOCIATE_STA: + if (tv[0][0]->len < sizeof(struct tlv_vendor_disassociate_sta)) { + warn("%s: invalid length of vendor Disassociate STA TLV\n", __func__); + return -1; + } + return handle_vendor_specific_disassociate_sta(agent, + (struct tlv_vendor_disassociate_sta *)tlv_impl); + default: + warn("%s: unknown type of vendor TLV\n", __func__); + break; + } + + return -1; +} +#endif diff --git a/src/vendor.h b/src/vendor.h new file mode 100644 index 0000000000000000000000000000000000000000..d8203e9295c8c293d4fba28a07fde8ad7a813e6a --- /dev/null +++ b/src/vendor.h @@ -0,0 +1,50 @@ +#ifndef VENDOR_H +#define VENDOR_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)); + +int agent_gen_vendor_specific_probe_req_tlv(struct agent *a, + struct cmdu_buff *frm, uint8_t *sta_mac, uint8_t *bssid, uint8_t rcpi); + +struct cmdu_buff *agent_gen_vendor_specific_probe_req( + struct agent *a, + uint8_t *sta_mac, + uint8_t *bssid, + uint8_t rcpi); + +int handle_vendor_extension(void *agent, struct cmdu_buff *rx_cmdu, struct node *n); + +#endif + +#endif /* VENDOR_H */