diff --git a/src/common/binding/ieee80211/ieee80211.c b/src/common/binding/ieee80211/ieee80211.c index b925341..9f0289e 100644 --- a/src/common/binding/ieee80211/ieee80211.c +++ b/src/common/binding/ieee80211/ieee80211.c @@ -456,9 +456,11 @@ int ieee80211_create_beacon(uint8_t* buffer, int length, struct ieee80211_beacon params->headbeacon = buffer; /* Management header frame */ - header->framecontrol = IEEE80211_FRAME_CONTROL(IEEE80211_FRAMECONTROL_TYPE_MGMT, IEEE80211_FRAMECONTROL_MGMT_SUBTYPE_BEACON); + header->framecontrol = IEEE80211_FRAME_CONTROL(IEEE80211_FRAMECONTROL_TYPE_MGMT, + IEEE80211_FRAMECONTROL_MGMT_SUBTYPE_BEACON); header->durationid = __cpu_to_le16(0); memset(header->da, 0xff, ETH_ALEN); + memcpy(header->sa, params->bssid, ETH_ALEN); memcpy(header->bssid, params->bssid, ETH_ALEN); header->sequencecontrol = __cpu_to_le16(0); @@ -522,6 +524,16 @@ int ieee80211_create_beacon(uint8_t* buffer, int length, struct ieee80211_beacon pos += result; params->tailbeaconlength += result; + log_printf(LOG_DEBUG, "IEEE80211: Beacon IE length: %d", params->beacon_ies_len); + if (params->beacon_ies_len) { + log_hexdump(LOG_DEBUG, "IEEE80211: Beacon IEs", + params->beacon_ies, params->beacon_ies_len); + + memcpy(pos, params->beacon_ies, params->beacon_ies_len); + pos += params->beacon_ies_len; + params->tailbeaconlength += params->beacon_ies_len; + } + /* Probe Response offload */ if (params->flags & IEEE80221_CREATE_BEACON_FLAGS_PROBE_RESPONSE_OFFLOAD) { struct ieee80211_probe_response_params proberesponseparams; @@ -547,6 +559,16 @@ int ieee80211_create_beacon(uint8_t* buffer, int length, struct ieee80211_beacon /* */ pos += params->proberesponseoffloadlength; + + log_printf(LOG_DEBUG, "IEEE80211: Probe Response IE length: %d", params->response_ies_len); + if (params->response_ies_len) { + log_hexdump(LOG_DEBUG, "IEEE80211: Probe Response IEs", + params->response_ies, params->response_ies_len); + + memcpy(pos, params->response_ies, params->response_ies_len); + pos += params->response_ies_len; + params->proberesponseoffloadlength += params->response_ies_len; + } } return (int)(pos - buffer); @@ -624,9 +646,20 @@ int ieee80211_create_probe_response(uint8_t* buffer, int length, struct ieee8021 return -1; } - /*pos += result;*/ /* Comment for disable Dead inscrement Clang Analyzer warning */ + pos += result; responselength += result; + log_printf(LOG_DEBUG, "IEEE80211: Probe Response IE length: %d", params->response_ies_len); + if (params->response_ies_len) { + log_hexdump(LOG_DEBUG, "IEEE80211: Response IEs", + params->response_ies, params->response_ies_len); + + memcpy(pos, params->response_ies, params->response_ies_len); + /* pos += params->response_ies_len; */ /* Comment for disable Dead inscrement Clang Analyzer warning */ + responselength += params->response_ies_len; + } + + return responselength; } diff --git a/src/common/binding/ieee80211/ieee80211.h b/src/common/binding/ieee80211/ieee80211.h index 649c071..5a4b93c 100644 --- a/src/common/binding/ieee80211/ieee80211.h +++ b/src/common/binding/ieee80211/ieee80211.h @@ -519,6 +519,11 @@ struct ieee80211_beacon_params { uint32_t mode; uint8_t erpinfo; + int beacon_ies_len; + uint8_t *beacon_ies; + int response_ies_len; + uint8_t *response_ies; + uint8_t* proberesponseoffload; int proberesponseoffloadlength; }; @@ -542,6 +547,9 @@ struct ieee80211_probe_response_params { uint32_t mode; uint8_t erpinfo; + + int response_ies_len; + uint8_t *response_ies; }; int ieee80211_create_probe_response(uint8_t* buffer, int length, struct ieee80211_probe_response_params* params); diff --git a/src/common/capwap_element_80211_ie.c b/src/common/capwap_element_80211_ie.c index b2f9a10..46859f3 100644 --- a/src/common/capwap_element_80211_ie.c +++ b/src/common/capwap_element_80211_ie.c @@ -56,6 +56,7 @@ static void* capwap_80211_ie_element_parsing(capwap_message_elements_handle hand data->ie = (uint8_t*)capwap_alloc(data->ielength); func->read_block(handle, data->ie, data->ielength); + log_printf(LOG_DEBUG, "802.11 IE flags: %02x (%p)", data->flags, &data->flags); return data; } diff --git a/src/wtp/binding/ieee80211/wifi_drivers.c b/src/wtp/binding/ieee80211/wifi_drivers.c index 7965f76..1c113ce 100644 --- a/src/wtp/binding/ieee80211/wifi_drivers.c +++ b/src/wtp/binding/ieee80211/wifi_drivers.c @@ -419,7 +419,7 @@ static void wifi_wlan_receive_station_mgmt_probe_request(struct wifi_wlan* wlan, int nowaitack; int responselength; struct ieee80211_ie_items ieitems; - struct ieee80211_probe_response_params ieee80211_params; + struct ieee80211_probe_response_params params; /* Information Elements packet length */ ielength = length - (sizeof(struct ieee80211_header) + sizeof(frame->proberequest)); @@ -444,19 +444,21 @@ static void wifi_wlan_receive_station_mgmt_probe_request(struct wifi_wlan* wlan, } /* Create probe response */ - memset(&ieee80211_params, 0, sizeof(struct ieee80211_probe_response_params)); - memcpy(ieee80211_params.bssid, wlan->address, MACADDRESS_EUI48_LENGTH); - memcpy(ieee80211_params.station, frame->sa, MACADDRESS_EUI48_LENGTH); - ieee80211_params.beaconperiod = wlan->device->beaconperiod; - ieee80211_params.capability = wifi_wlan_check_capability(wlan, wlan->capability); - ieee80211_params.ssid = wlan->ssid; - memcpy(ieee80211_params.supportedrates, wlan->device->supportedrates, wlan->device->supportedratescount); - ieee80211_params.supportedratescount = wlan->device->supportedratescount; - ieee80211_params.mode = wlan->device->currentfrequency.mode; - ieee80211_params.erpinfo = ieee80211_get_erpinfo(wlan->device->currentfrequency.mode, wlan->device->olbc, wlan->device->stationsnonerpcount, wlan->device->stationsnoshortpreamblecount, wlan->device->shortpreamble); - ieee80211_params.channel = wlan->device->currentfrequency.channel; + memset(¶ms, 0, sizeof(struct ieee80211_probe_response_params)); + memcpy(params.bssid, wlan->address, MACADDRESS_EUI48_LENGTH); + memcpy(params.station, frame->sa, MACADDRESS_EUI48_LENGTH); + params.beaconperiod = wlan->device->beaconperiod; + params.capability = wifi_wlan_check_capability(wlan, wlan->capability); + params.ssid = wlan->ssid; + memcpy(params.supportedrates, wlan->device->supportedrates, wlan->device->supportedratescount); + params.supportedratescount = wlan->device->supportedratescount; + params.mode = wlan->device->currentfrequency.mode; + params.erpinfo = ieee80211_get_erpinfo(wlan->device->currentfrequency.mode, wlan->device->olbc, wlan->device->stationsnonerpcount, wlan->device->stationsnoshortpreamblecount, wlan->device->shortpreamble); + params.channel = wlan->device->currentfrequency.channel; + params.response_ies = wlan->response_ies; + params.response_ies_len = wlan->response_ies_len; - responselength = ieee80211_create_probe_response(g_bufferIEEE80211, sizeof(g_bufferIEEE80211), &ieee80211_params); + responselength = ieee80211_create_probe_response(g_bufferIEEE80211, sizeof(g_bufferIEEE80211), ¶ms); if (responselength < 0) { return; } @@ -1512,6 +1514,57 @@ struct wifi_wlan* wifi_wlan_create(struct wifi_device* device, const char* ifnam return wlan; } +/* Build 802.11 Information Elements from CAPWAP Message Elements */ +static void build_80211_ie(uint8_t radioid, uint8_t wlanid, uint8_t type, + struct capwap_array *ie, + uint8_t **ptr, int *len) +{ + uint8_t buffer[IEEE80211_MTU]; + ssize_t space = sizeof(buffer); + uint8_t *pos = buffer; + int i; + + ASSERT(ptr); + ASSERT(len); + + log_printf(LOG_DEBUG, "WIFI 802.11: IE: %d:%d %02x, %p", + radioid, wlanid, type, ie); + + *len = 0; + *ptr = NULL; + + if (!ie) + return; + + for (i = 0; i < ie->count; i++) { + struct capwap_80211_ie_element *e = + *(struct capwap_80211_ie_element **)capwap_array_get_item_pointer(ie, i); + + log_printf(LOG_DEBUG, "WIFI 802.11: IE: %d:%d %02x (%p)", + radioid, wlanid, e->flags, &e->flags); + + if (e->radioid != radioid || + e->wlanid != wlanid || + !(e->flags & type)) + continue; + + /* not enough space left */ + if (e->ielength > space) + continue; + + memcpy(pos, e->ie, e->ielength); + pos += e->ielength; + space -= e->ielength; + } + + *len = pos - buffer; + if (*len > 0) { + *ptr = malloc(*len); + if (*ptr) + memcpy(*ptr, buffer, *len); + } +} + /* */ int wifi_wlan_startap(struct wifi_wlan* wlan, struct wlan_startap_params* params) { int result; @@ -1535,6 +1588,15 @@ int wifi_wlan_startap(struct wifi_wlan* wlan, struct wlan_startap_params* params wlan->radioid = params->radioid; wlan->wlanid = params->wlanid; + build_80211_ie(wlan->radioid, wlan->wlanid, + CAPWAP_IE_BEACONS_ASSOCIATED, + params->ie, + &wlan->beacon_ies, &wlan->beacon_ies_len); + build_80211_ie(wlan->radioid, wlan->wlanid, + CAPWAP_IE_PROBE_RESPONSE_ASSOCIATED, + params->ie, + &wlan->response_ies, &wlan->response_ies_len); + /* Start AP */ result = wlan->device->instance->ops->wlan_startap(wlan); if (!result) { @@ -1555,6 +1617,11 @@ void wifi_wlan_stopap(struct wifi_wlan* wlan) { /* Stop AP */ wlan->device->instance->ops->wlan_stopap(wlan); + free(wlan->beacon_ies); + wlan->beacon_ies = NULL; + free(wlan->response_ies); + wlan->response_ies = NULL; + /* */ if (wlan->flags & WIFI_WLAN_RUNNING) { wlan->device->wlanactive--; diff --git a/src/wtp/binding/ieee80211/wifi_drivers.h b/src/wtp/binding/ieee80211/wifi_drivers.h index 9a244b5..82ee720 100644 --- a/src/wtp/binding/ieee80211/wifi_drivers.h +++ b/src/wtp/binding/ieee80211/wifi_drivers.h @@ -100,6 +100,8 @@ struct wlan_startap_params { uint8_t authmode; uint8_t macmode; uint8_t tunnelmode; + + struct capwap_array *ie; }; @@ -309,6 +311,11 @@ struct wifi_wlan { unsigned long maxstationscount; uint32_t aidbitfield[IEEE80211_AID_BITFIELD_SIZE]; + + int beacon_ies_len; + uint8_t *beacon_ies; + int response_ies_len; + uint8_t *response_ies; }; /* Station handle */ diff --git a/src/wtp/binding/ieee80211/wifi_nl80211.c b/src/wtp/binding/ieee80211/wifi_nl80211.c index ece4c0f..8d57d69 100644 --- a/src/wtp/binding/ieee80211/wifi_nl80211.c +++ b/src/wtp/binding/ieee80211/wifi_nl80211.c @@ -2,6 +2,8 @@ #include "capwap_array.h" #include "capwap_list.h" #include "capwap_element.h" +#include "capwap_element_80211_ie.h" + #include #include #include @@ -638,59 +640,92 @@ static int nl80211_wlan_getfdevent(struct wifi_wlan* wlan, struct pollfd* fds, s static int nl80211_wlan_setbeacon(struct wifi_wlan* wlan) { int result; struct nl_msg* msg; + uint8_t cmd = NL80211_CMD_START_AP; struct nl80211_wlan_handle* wlanhandle; - struct ieee80211_beacon_params ieee80211_params; + struct ieee80211_beacon_params params; uint8_t buffer[IEEE80211_MTU]; + int beacon_set; /* */ wlanhandle = (struct nl80211_wlan_handle*)wlan->handle; /* Create beacon packet */ - memset(&ieee80211_params, 0, sizeof(struct ieee80211_beacon_params)); - memcpy(ieee80211_params.bssid, wlan->address, ETH_ALEN); - ieee80211_params.beaconperiod = wlan->device->beaconperiod; - ieee80211_params.capability = wifi_wlan_check_capability(wlan, wlan->capability); - ieee80211_params.ssid = wlan->ssid; - ieee80211_params.ssid_hidden = wlan->ssid_hidden; - memcpy(ieee80211_params.supportedrates, wlan->device->supportedrates, wlan->device->supportedratescount); - ieee80211_params.supportedratescount = wlan->device->supportedratescount; - ieee80211_params.mode = wlan->device->currentfrequency.mode; - ieee80211_params.erpinfo = ieee80211_get_erpinfo(wlan->device->currentfrequency.mode, wlan->device->olbc, wlan->device->stationsnonerpcount, wlan->device->stationsnoshortpreamblecount, wlan->device->shortpreamble); - ieee80211_params.channel = wlan->device->currentfrequency.channel; + memset(¶ms, 0, sizeof(struct ieee80211_beacon_params)); + memcpy(params.bssid, wlan->address, ETH_ALEN); + params.beaconperiod = wlan->device->beaconperiod; + params.capability = wifi_wlan_check_capability(wlan, wlan->capability); + params.ssid = wlan->ssid; + params.ssid_hidden = wlan->ssid_hidden; + memcpy(params.supportedrates, wlan->device->supportedrates, wlan->device->supportedratescount); + params.supportedratescount = wlan->device->supportedratescount; + params.mode = wlan->device->currentfrequency.mode; + params.erpinfo = ieee80211_get_erpinfo(wlan->device->currentfrequency.mode, wlan->device->olbc, wlan->device->stationsnonerpcount, wlan->device->stationsnoshortpreamblecount, wlan->device->shortpreamble); + params.channel = wlan->device->currentfrequency.channel; + + params.beacon_ies = wlan->beacon_ies; + params.beacon_ies_len = wlan->beacon_ies_len; + params.response_ies = wlan->response_ies; + params.response_ies_len = wlan->response_ies_len; /* Enable probe response offload only in CAPWAP Local Mac */ - if ((wlan->macmode == CAPWAP_ADD_WLAN_MACMODE_LOCAL) && (wlan->device->capability->capability & WIFI_CAPABILITY_FLAGS_PROBE_RESPONSE_OFFLOAD)) { - ieee80211_params.flags |= IEEE80221_CREATE_BEACON_FLAGS_PROBE_RESPONSE_OFFLOAD; + if ((wlan->macmode == CAPWAP_ADD_WLAN_MACMODE_LOCAL) && + (wlan->device->capability->capability & WIFI_CAPABILITY_FLAGS_PROBE_RESPONSE_OFFLOAD)) { + params.flags |= IEEE80221_CREATE_BEACON_FLAGS_PROBE_RESPONSE_OFFLOAD; } /* */ - result = ieee80211_create_beacon(buffer, sizeof(buffer), &ieee80211_params); + result = ieee80211_create_beacon(buffer, sizeof(buffer), ¶ms); if (result < 0) { return -1; } - /* */ - msg = nlmsg_alloc(); - if (!msg) { - return -1; - } + beacon_set = !!(wlan->flags & WIFI_WLAN_SET_BEACON); - /* */ - genlmsg_put(msg, 0, 0, wlanhandle->devicehandle->globalhandle->nl80211_id, 0, 0, ((wlan->flags & WIFI_WLAN_SET_BEACON) ? NL80211_CMD_SET_BEACON : NL80211_CMD_START_AP), 0); + log_printf(LOG_DEBUG, "nl80211: Set beacon (beacon_set=%d)", + beacon_set); + if (beacon_set) + cmd = NL80211_CMD_SET_BEACON; + + log_hexdump(LOG_DEBUG, "nl80211: Beacon head", + params.headbeacon, params.headbeaconlength); + log_hexdump(LOG_DEBUG, "nl80211: Beacon tail", + params.tailbeacon, params.tailbeaconlength); + log_printf(LOG_DEBUG, "nl80211: ifindex=%d", wlan->virtindex); + log_printf(LOG_DEBUG, "nl80211: beacon_int=%d", wlan->device->beaconperiod); + log_printf(LOG_DEBUG, "nl80211: dtim_period=%d", wlan->device->dtimperiod); + log_hexdump(LOG_DEBUG, "nl80211: ssid", + (uint8_t *)wlan->ssid, strlen(wlan->ssid)); + + msg = nlmsg_alloc(); + if (!msg) + return -1; + + genlmsg_put(msg, 0, 0, wlanhandle->devicehandle->globalhandle->nl80211_id, 0, 0, cmd, 0); nla_put_u32(msg, NL80211_ATTR_IFINDEX, wlan->virtindex); - nla_put(msg, NL80211_ATTR_BEACON_HEAD, ieee80211_params.headbeaconlength, ieee80211_params.headbeacon); - nla_put(msg, NL80211_ATTR_BEACON_TAIL, ieee80211_params.tailbeaconlength, ieee80211_params.tailbeacon); + nla_put(msg, NL80211_ATTR_BEACON_HEAD, params.headbeaconlength, params.headbeacon); + nla_put(msg, NL80211_ATTR_BEACON_TAIL, params.tailbeaconlength, params.tailbeacon); nla_put_u32(msg, NL80211_ATTR_BEACON_INTERVAL, wlan->device->beaconperiod); nla_put_u32(msg, NL80211_ATTR_DTIM_PERIOD, wlan->device->dtimperiod); nla_put(msg, NL80211_ATTR_SSID, strlen(wlan->ssid), wlan->ssid); - nla_put_u32(msg, NL80211_ATTR_HIDDEN_SSID, (wlan->ssid_hidden ? NL80211_HIDDEN_SSID_ZERO_LEN : NL80211_HIDDEN_SSID_NOT_IN_USE)); + + if ((wlan->device->capability->capability & WIFI_CAPABILITY_FLAGS_PROBE_RESPONSE_OFFLOAD) && + (params.proberesponseoffloadlength > 0)) { + log_hexdump(LOG_DEBUG, "nl80211: proberesp (offload)", + params.proberesponseoffload, params.proberesponseoffloadlength); + nla_put(msg, NL80211_ATTR_PROBE_RESP, params.proberesponseoffloadlength, params.proberesponseoffload); + } + + if (!wlan->ssid_hidden) { + log_printf(LOG_DEBUG, "nl80211: hidden SSID not in use"); + nla_put_u32(msg, NL80211_ATTR_HIDDEN_SSID, NL80211_HIDDEN_SSID_NOT_IN_USE); + } else { + log_printf(LOG_DEBUG, "nl80211: hidden SSID zero len"); + nla_put_u32(msg, NL80211_ATTR_HIDDEN_SSID, NL80211_HIDDEN_SSID_ZERO_LEN); + } + nla_put_u32(msg, NL80211_ATTR_AUTH_TYPE, ((wlan->authmode == CAPWAP_ADD_WLAN_AUTHTYPE_WEP) ? NL80211_AUTHTYPE_SHARED_KEY : NL80211_AUTHTYPE_OPEN_SYSTEM)); nla_put_flag(msg, NL80211_ATTR_CONTROL_PORT_NO_ENCRYPT); - if ((wlan->device->capability->capability & WIFI_CAPABILITY_FLAGS_PROBE_RESPONSE_OFFLOAD) && (ieee80211_params.proberesponseoffloadlength > 0)) { - nla_put(msg, NL80211_ATTR_PROBE_RESP, ieee80211_params.proberesponseoffloadlength, ieee80211_params.proberesponseoffload); - } - /* Start AP */ result = nl80211_send_and_recv_msg(wlanhandle->devicehandle->globalhandle, msg, NULL, NULL); if (result) { @@ -711,7 +746,7 @@ static int nl80211_wlan_setbeacon(struct wifi_wlan* wlan) { nla_put_u32(msg, NL80211_ATTR_IFINDEX, wlan->virtindex); /* */ - nla_put_u8(msg, NL80211_ATTR_BSS_CTS_PROT, ((ieee80211_params.erpinfo & IEEE80211_ERP_INFO_USE_PROTECTION) ? 1 : 0)); + nla_put_u8(msg, NL80211_ATTR_BSS_CTS_PROT, ((params.erpinfo & IEEE80211_ERP_INFO_USE_PROTECTION) ? 1 : 0)); nla_put_u8(msg, NL80211_ATTR_BSS_SHORT_PREAMBLE, ((!wlan->device->stationsnoshortpreamblecount && wlan->device->shortpreamble) ? 1 : 0)); //nla_put_u16(msg, NL80211_ATTR_BSS_HT_OPMODE, ???); //nla_put_u8(msg, NL80211_ATTR_AP_ISOLATE, ???); @@ -1283,9 +1318,11 @@ static int cb_get_phydevice_capability(struct nl_msg* msg, void* data) { } if (tb_msg[NL80211_ATTR_PROBE_RESP_OFFLOAD]) { + log_printf(LOG_DEBUG, "nl80211: Supports Probe Response offload in AP mode"); capability->capability |= WIFI_CAPABILITY_FLAGS_PROBE_RESPONSE_OFFLOAD; /* TODO check offload protocol support */ - } + } else + log_printf(LOG_DEBUG, "nl80211: Does not support Probe Response offload in AP mode"); /* Cipher supported */ if (tb_msg[NL80211_ATTR_CIPHER_SUITES]) { diff --git a/src/wtp/kmod/capwap.c b/src/wtp/kmod/capwap.c index f1d714a..9dfed48 100644 --- a/src/wtp/kmod/capwap.c +++ b/src/wtp/kmod/capwap.c @@ -431,7 +431,6 @@ int sc_capwap_create(struct sc_capwap_session *session) goto error; return err; - error: if (session->socket) udp_tunnel_sock_release(session->socket); diff --git a/src/wtp/wtp_radio.c b/src/wtp/wtp_radio.c index 92f5d48..d4d5646 100644 --- a/src/wtp/wtp_radio.c +++ b/src/wtp/wtp_radio.c @@ -687,7 +687,7 @@ uint32_t wtp_radio_create_wlan(struct capwap_parsed_packet* packet, struct capwa params.authmode = addwlan->authmode; params.macmode = addwlan->macmode; params.tunnelmode = addwlan->tunnelmode; - /* TODO (struct capwap_array*)capwap_get_message_element_data(packet, CAPWAP_ELEMENT_80211_IE) */ + params.ie = (struct capwap_array *)capwap_get_message_element_data(packet, CAPWAP_ELEMENT_80211_IE); /* Start AP */ if (wifi_wlan_startap(wlanpool->wlanhandle, ¶ms)) {