From e974b3ae2db4295ec232e4a2f23f3c56806c8ea8 Mon Sep 17 00:00:00 2001 From: 7u83 <7u83@mail.ru> Date: Tue, 20 Sep 2022 08:31:04 +0200 Subject: [PATCH] Work on wifi implemntation --- src/ac/wtpman.c | 44 ++++++++++++-- src/cw/cw.h | 8 +++ src/cw/dbg.c | 134 ++++++++---------------------------------- src/cw/dbg.h | 17 +++--- src/cw/dot11.h | 23 +++++++- src/cw/dot11_inline.c | 13 +++- src/wtp/Makefile | 7 ++- src/wtp/nlt.c | 44 +++++++++----- src/wtp/wtp_main.c | 12 +++- 9 files changed, 155 insertions(+), 147 deletions(-) diff --git a/src/ac/wtpman.c b/src/ac/wtpman.c index 413c1941..01da1efd 100644 --- a/src/ac/wtpman.c +++ b/src/ac/wtpman.c @@ -317,19 +317,51 @@ int run_update(struct wtpman *wtpman) static int dataman_process_msg(struct cw_Conn *nc, uint8_t * rawmsg, int len, struct sockaddr *from) { + char rframe[1000]; int offs = cw_get_hdr_msg_offset(rawmsg); - uint8_t * dot11frame = rawmsg + offs; - int dot11len = len-offs; - cw_dbg_dot11_frame(dot11frame,dot11len); + uint8_t * frame = rawmsg + offs; + int frame_len = len-offs; + cw_dbg_dot11_frame(frame,frame_len); - char frame[1000]; - dot11_init_assoc_resp(frame); +/* dot11_init_assoc_resp(frame); dot11_copy_mac(dot11_get_sa(dot11frame),dot11_get_da(frame)); dot11_copy_mac(dot11_get_bssid(dot11frame),dot11_get_bssid(frame)); dot11_copy_mac(dot11_get_da(dot11frame),dot11_get_sa(frame)); dot11_set_seq(frame,0); - +*/ + if ( dot11_get_type_and_subtype(frame) == DOT11_ASSOC_REQ){ + int l; + uint8_t rates[] = { + 12,0x82,0x84,0x8b,0x96,0x0c,0x12,0x18,0x24,0x30,0x48,0x60,0x6c + }; + cw_dbg(DBG_X, "there is an assoc request!"); + + uint8_t rframe[1000]; + nc->mtu=800; + + + dot11_init_assoc_resp(rframe); + dot11_set_duration(rframe,100); + + dot11_copy_mac(dot11_get_sa(frame),dot11_get_da(rframe)); + dot11_copy_mac(dot11_get_bssid(frame),dot11_get_bssid(rframe)); + dot11_copy_mac(dot11_get_da(frame),dot11_get_sa(rframe)); + dot11_set_seq(rframe,dot11_get_seq(frame)); + dot11_assoc_resp_set_cap(rframe,dot11_assoc_req_get_cap(frame)); + dot11_assoc_resp_set_status_code(rframe,0); + dot11_assoc_resp_set_assoc_id(rframe,17); + l=24+6; + l+=dot11_put_supported_rates(rframe+l,rates); + cw_dbg_dot11_frame(rframe,l); + + uint8_t buf[1024]; + int hlen; + hlen = cw_init_capwap_packet(buf,1,0,NULL,NULL); + cw_set_hdr_flags(buf, CAPWAP_FLAG_HDR_T, 1); + cw_send_capwap_packet(nc,buf,hlen,rframe,l); + + } return 0; diff --git a/src/cw/cw.h b/src/cw/cw.h index fce80c8e..788d6b95 100644 --- a/src/cw/cw.h +++ b/src/cw/cw.h @@ -565,6 +565,14 @@ int cw_out_generic_walk(struct cw_ElemHandler * handler, struct cw_ElemHandlerPa int cw_out_generic0(struct cw_ElemHandler * handler, struct cw_ElemHandlerParams * params , uint8_t * dst,const char *key); + + + +int cw_init_capwap_packet( uint8_t * buf, int wbid ,int rid, bstr_t rmac, bstr_t wd); +int cw_send_capwap_packet(struct cw_Conn * conn, uint8_t * buf, int hlen, uint8_t * data, int len); + + + /** *@} */ diff --git a/src/cw/dbg.c b/src/cw/dbg.c index 9483b675..5f8924d3 100644 --- a/src/cw/dbg.c +++ b/src/cw/dbg.c @@ -38,19 +38,14 @@ *@{ */ - /* - * @defgroup DebugFunctions Debug Functions - * @{ - */ - uint32_t cw_dbg_opt_display = DBG_DISP_COLORS; /** - * Current debug level + * Contains all debuglevels currently set */ -//static uint32_t cw_dbg_opt_level = 0; -// static struct mavl * cw_dbg_opt_level = NULL; + + static int dbg_cmp(const void *a, const void*b) { return (*((int*)a)-*((int*)b)); @@ -91,13 +86,6 @@ static const char * dbg_level_elem_all[] = { static const char * dbg_level_std[] = { "msg","elem","msg_err", "elem_err", "pkt_err", "rfc", "warn", "state", "info", NULL -/* - DBG_MSG_IN, DBG_MSG_OUT, - DBG_ELEM_IN, DBG_ELEM_OUT, - DBG_MSG_ERR, DBG_ELEM_ERR, - DBG_PKT_ERR, DBG_RFC, DBG_WARN, - DBG_STATE, DBG_INFO, - 0*/ }; @@ -159,14 +147,6 @@ struct cw_DbgStr cw_dbg_strings[] = { { 0, NULL } }; -/** - *@} - */ - - - - - static struct cw_StrListElem theme0[] = { @@ -217,7 +197,6 @@ static struct cw_StrListElem * color_on = theme0; struct cw_StrListElem color_ontext[] = { -/* {DBG_ELEM_DMP, "\x1b[37m"},*/ {DBG_ELEM_DMP_OUT, ANSI_BBLACK ANSI_ITALIC}, {DBG_ELEM_DMP_IN, ANSI_BBLACK}, @@ -296,22 +275,36 @@ const char *get_dbg_color_ontext(int level) * @param level Level to check * @return 0 if leveln is not set, otherwise level is set */ - int cw_dbg_is_level(int level) { if (cw_dbg_opt_level == NULL) return 0; return mavl_get(cw_dbg_opt_level,&level) == NULL ? 0:1; +} +/** + * Set debug level + * @param level debug level to set, allowed values are enumberated in #cw_dbg_levels structure. + * @param on 1: turns the specified debug level on, 0: turns the specified debug level off. + * + * To check if a specific debug level is set, call #cw_dbg_is_level. + */ +void cw_dbg_set_level (int level, int on) +{ + int exists; -// if (level > 1 && (level &1)) - return 1; + if (cw_dbg_opt_level == NULL){ + cw_dbg_opt_level = mavl_create(dbg_cmp,NULL,sizeof(int)); + if (cw_dbg_opt_level == NULL) + return; + } -/* if (level >= DBG_ALL ){ - return 1; - }*/ -// return (cw_dbg_opt_level & (level)); + if (on){ + mavl_insert(cw_dbg_opt_level,&level,&exists); + } + else + mavl_del(cw_dbg_opt_level,&level); } @@ -331,48 +324,12 @@ static void cw_dbg_vlog_line(struct cw_LogWriter * writer, } writer->write(LOG_DEBUG,fbuf,args,writer); - } /** - * Put a list of missing mandatory message elements to debug output - */ -void cw_dbg_missing_mand(int level, struct cw_Conn *conn, int ** ml, int n, - int * a) -{ - /* -// if (!cw_dbg_is_level(DBG_MSG_ERR) || n == 0) -// return; -*/ - - - char buffer[2000]; -/* char *p = buffer; */ - int i; -/* char *delim = "";*/ - - if (!cw_dbg_is_level(level) || n == 0) - return; - -/* - // TODO XXXX -*/ - for (i = 0; i < n; i++) { -/* p += sprintf(p, "%s", delim); - delim = ", "; - p += sprintf(p, "%s", cw_strelemp(conn->actions, ml[i]->elem_id)); -*/ - } - cw_dbg(level, "Missing mandatory elements: [%s]", buffer); -} - - - - -/** - * Display a packet on for debugger + * Display a packet on debugger */ void cw_dbg_pkt(int level, struct cw_Conn *conn, uint8_t * packet, int len, struct sockaddr *from) @@ -558,46 +515,6 @@ void cw_dbg_elem(int level, struct cw_Conn *conn, int msg, -/** - * Set debug level - * @param level debug level to set, allowed values are enumberated in #cw_dbg_levels structure. - * @param on 1: turns the specified debug level on, 0: turns the specified debug level off. - */ - -void cw_dbg_set_level (int level, int on) -{ - int exists; - - if (cw_dbg_opt_level == NULL){ - cw_dbg_opt_level = mavl_create(dbg_cmp,NULL,sizeof(int)); - if (cw_dbg_opt_level == NULL) - return; - } - - if (on){ - mavl_insert(cw_dbg_opt_level,&level,&exists); - } - else - mavl_del(cw_dbg_opt_level,&level); - - -/* - switch (level) { - case DBG_ALL: - if (on) - cw_dbg_opt_level = 0xffffffff; - else - cw_dbg_opt_level = 0; - break; - default: - if (on) - cw_dbg_opt_level |= (level); - else - cw_dbg_opt_level &= (0xffffffff) ^ (level); - } - */ -} - int cw_dbg_set_level_from_str0(const char *level,int on) { @@ -728,4 +645,3 @@ void cw_dbg_dot11_frame(uint8_t * frame,int len) /**@}*/ -/**@}*/ diff --git a/src/cw/dbg.h b/src/cw/dbg.h index 43a0f8de..593b3b20 100644 --- a/src/cw/dbg.h +++ b/src/cw/dbg.h @@ -39,7 +39,7 @@ /** - *@addtogroup LOGDBG + *@addtogroup DBG *@{ */ @@ -82,9 +82,6 @@ enum cw_dbg_levels{ /** Error in msg elements */ DBG_ELEM_ERR, - /** hex dump elements */ - DBG_ELEM_DMP, - /** General infos, like CAPWAP state */ DBG_INFO, @@ -116,25 +113,27 @@ enum cw_dbg_levels{ /**Debug State machine */ DBG_STATE, - + /** Infos about nessage composing */ DBG_MSG_COMPOSE, + /** Debug Configuration updates */ DBG_CFG_UPDATES, + /** Debug Vendor elements */ DBG_ELEM_VNDR, - + DBG_X, DBG_MOD_DETAIL, DBG_ALL, - + /** Hexdump incoming msg elemenets */ DBG_ELEM_DMP_IN, + /** Hexdump outgoing msg elemenst */ DBG_ELEM_DMP_OUT, - }; #define DBG_MSG (DBG_MSG_IN | DBG_MSG_OUT) @@ -220,8 +219,6 @@ void cw_dbg_msg(int level,struct cw_Conn *conn, uint8_t * packet, int len,struct char * cw_dbg_mkdmp(const uint8_t * data, int len); //void cw_dbg_version_subelem(int level, const char *context, int subtype, // uint32_t vendor_id, const uint8_t * vstr, int len); -void cw_dbg_ktv_dump(mavl_t ktv, uint32_t dbglevel, - const char *header, const char *prefix, const char *footer ); /** * Set debug level diff --git a/src/cw/dot11.h b/src/cw/dot11.h index b1c455c5..a6d52e77 100644 --- a/src/cw/dot11.h +++ b/src/cw/dot11.h @@ -21,6 +21,7 @@ #endif #include "strlist.h" +#include "bstr.h" /** * @defgroup DOT11_FRAME_TYPES Frame Types @@ -238,7 +239,9 @@ int dot11_put_ssid(uint8_t *dst,uint8_t * ssid,int len); */ #define dot11_rate2float(rate) (((float)(rate))/2.0) -int dot11_put_supported_rates(uint8_t *dst, float *basic, float *rates); +int dot11_put_supported_rates(uint8_t *dst, bstr_t src); + +// float *basic, float *rates); int dot11_put_dsss_param_set(uint8_t *dst,int ch); @@ -295,10 +298,22 @@ extern struct cw_StrListElem dot11_names[]; */ #define dot11_assoc_resp_set_cap(frame,cap)\ dot11_set_word(dot11_get_body(frame),cap) +#define dot11_assoc_resp_get_cap(frame)\ + dot11_get_word(dot11_get_body(frame)) + + #define dot11_assoc_resp_set_status_code(frame,code)\ dot11_set_word(dot11_get_body(frame)+2,code) +#define dot11_assoc_resp_get_status_code(frame)\ + dot11_get_word(dot11_get_body(frame)+2) + + #define dot11_assoc_resp_set_assoc_id(frame,id)\ dot11_set_word(dot11_get_body(frame)+4,id) +#define dot11_assoc_resp_get_assoc_id(frame)\ + dot11_get_word(dot11_get_body(frame)+4) + + //#define dot11_assoce_resp_get_var_body(frame) // (get_frame_body(frame)+6) @@ -309,6 +324,12 @@ extern struct cw_StrListElem dot11_names[]; memcpy(dst,src,6); +struct cw_Dot11Elemenst { + + bstr_t supportet_rates; +}; + +int dot11_init_assoc_resp(uint8_t * dst); /** diff --git a/src/cw/dot11_inline.c b/src/cw/dot11_inline.c index ea7b6787..cdf53287 100644 --- a/src/cw/dot11_inline.c +++ b/src/cw/dot11_inline.c @@ -45,7 +45,9 @@ int dot11_put_ssid(uint8_t *dst,uint8_t * ssid,int len){ return len; } -int dot11_put_supported_rates(uint8_t *dst, float *basic, float *rates){ + + +int dot11_convert_supported_rates(uint8_t *dst, float *basic, float *rates){ uint8_t *d = dst+2; while(*basic != 0.0){ *d++ = 0x80 | dot11_float2rate(*basic); @@ -62,6 +64,15 @@ int dot11_put_supported_rates(uint8_t *dst, float *basic, float *rates){ } +int dot11_put_supported_rates(uint8_t * dst, bstr_t src) +{ + int l = bstr_len(src); + dot11_set_byte(dst,DOT11_ELEM_SUPPORTED_RATES); + memcpy(dst+1, src,l+1); + return l+2; +} + + int dot11_put_dsss_param_set(uint8_t *dst,int ch) { dot11_set_byte(dst,DOT11_ELEM_DSSS_PARAM_SET); diff --git a/src/wtp/Makefile b/src/wtp/Makefile index 9de3ddea..13189625 100644 --- a/src/wtp/Makefile +++ b/src/wtp/Makefile @@ -5,9 +5,9 @@ OBJDIR=./o LIBDIR := ../../lib LIBARCHDIR := $(LIBDIR)/$(KERNEL)/$(ARCH) -CFLAGS+=-I../ -DUSE_OPENSSL -I../../include +CFLAGS+=-I../ -DUSE_OPENSSL -I../../include -I/usr/include/libnl3 LDFLAGS+=-L$(LIBARCHDIR) -L/usr/local/lib -LIBS+=-lcw -lnettle -lssl -lcrypto -ldl -lpthread -lmavl -lwifi +LIBS+=-lcw -lnettle -lssl -lcrypto -ldl -lpthread -lmavl -lwifi -lnl-3 -lnl-genl-3 SOURCES=\ wtp_main.c\ @@ -15,6 +15,7 @@ SOURCES=\ join.c\ configure.c\ run.c\ - changestate.c + changestate.c \ + nlt.c include ../prog.mk diff --git a/src/wtp/nlt.c b/src/wtp/nlt.c index fcf7d2c1..ebac2e6b 100644 --- a/src/wtp/nlt.c +++ b/src/wtp/nlt.c @@ -1,16 +1,17 @@ -#include "netlink/netlink.h" -#include "netlink/genl/genl.h" -#include "netlink/genl/ctrl.h" +#include +#include +#include #include #include "cw/log.h" #include "cw/dbg.h" -#include "cw/avltree.h" + +#include #include "nlt.h" -/* + static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg) { @@ -32,21 +33,22 @@ static int wiphylist_cmp(const void * d1,const void *d2) } -struct avltree * wiphylist_create() +struct mavl * wiphylist_create() { - return avltree_create(wiphylist_cmp,0); + return mavl_create(wiphylist_cmp,0,sizeof(struct nlt_wiphyinfo)); } -struct nlt_wiphyinfo * wiphylist_get( struct avltree * l,int idx) +struct nlt_wiphyinfo * wiphylist_get( struct mavl * l,int idx) { //return avltree_get(l); } -struct nlt_wiphyinfo * nlt_wiphylist_add(struct avltree * t, struct nlt_wiphyinfo * wi) +struct nlt_wiphyinfo * nlt_wiphylist_add(struct mavl * t, struct nlt_wiphyinfo * wi) { - return avltree_add(t,wi); + int exists; + return mavl_insert(t,wi,exists); } @@ -258,7 +260,7 @@ static int nlCallback(struct nl_msg *msg, void *arg) genlmsg_attrlen(ghdr, 0), NULL); if (rc < 0) { - cw_dbg(DBG_DRV_ERR, "nla_parse failed: %d %d", rc, nl_geterror(rc)); + cw_dbg(DBG_X, "nla_parse failed: %d %d", rc, nl_geterror(rc)); return NL_SKIP; } @@ -267,7 +269,7 @@ static int nlCallback(struct nl_msg *msg, void *arg) switch (cmd) { case NL80211_CMD_NEW_WIPHY: - add_wiphy_data(msgattribs, arg); + //add_wiphy_data(msgattribs, arg); break; // case NL80211_CMD_NEW_INTERFACE: // add_interface_data(msgattribs); @@ -337,7 +339,6 @@ static int get_wiphy_info_cb(struct nl_msg * msg,void * arg) int nlt_get_wiphy_list(struct nl_sock *sk) { - struct nlt_wiphyinfo ** wi = malloc (sizeof(struct nlt_wiphyinfo *)*NLT_MAX_WIPHYINDEX); if (wi==0) return 0; @@ -347,9 +348,9 @@ int nlt_get_wiphy_list(struct nl_sock *sk) struct nl_msg * msg = nlt_nl_msg_new(sk,NL80211_CMD_GET_WIPHY,NLM_F_DUMP); nl_send_auto(sk, msg); struct nl_cb *nl_cb = get_nl_cb(get_wiphy_info_cb,wi); -// while(1){ + while(1){ int nlr = nl_recvmsgs(sk, nl_cb); -// } + } int i; @@ -364,4 +365,15 @@ int nlt_get_wiphy_list(struct nl_sock *sk) } -*/ +nlt_test() +{ + struct nl_sock *nl; + nl = nl_socket_alloc(); + if (!nl) { + fprintf(stderr, "Failed to allocate netlink socket.\n"); + return -ENOMEM; + } + + nlt_get_wiphy_list(nl); + +} diff --git a/src/wtp/wtp_main.c b/src/wtp/wtp_main.c index 61348f73..ddcda18e 100644 --- a/src/wtp/wtp_main.c +++ b/src/wtp/wtp_main.c @@ -100,6 +100,11 @@ int test() size_t len; f=(uint8_t*)cw_load_file("wificap-002",&len); cw_dbg(DBG_X, "Loaded %d bytes",len); + int l; + + uint8_t rates[] = { + 12,0x82,0x84,0x8b,0x96,0x0c,0x12,0x18,0x24,0x30,0x48,0x60,0x6c + }; // static int got_radiotap = 0; // struct libwifi_frame frame = {0}; @@ -145,8 +150,12 @@ int test() dot11_assoc_resp_set_cap(rframe,dot11_assoc_req_get_cap(frame)); dot11_assoc_resp_set_status_code(rframe,0); dot11_assoc_resp_set_assoc_id(rframe,17); + l=24+6; - cw_dbg_dot11_frame(rframe,24+6); + + l+=dot11_put_supported_rates(rframe+l,rates); + + cw_dbg_dot11_frame(rframe,l); @@ -205,6 +214,7 @@ int main (int argc, char **argv) } test(); + nlt_test(); stop();