--- src/drivers/driver_nl80211.c | 238 ++++++++++++++++++++++++++++++++++++++++++ 1 files changed, 238 insertions(+), 0 deletions(-) diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c index 72e3e20..97d069f 100644 --- a/src/drivers/driver_nl80211.c +++ b/src/drivers/driver_nl80211.c @@ -180,6 +180,17 @@ struct nl80211_global { int if_add_ifindex; }; +struct nl80211_wiphy_data { + struct dl_list list; + struct dl_list bsss; + struct dl_list drvs; + + struct nl80211_handles nl_beacons; + struct nl_cb *nl_cb; + + int wiphy_idx; +}; + struct i802_bss { struct wpa_driver_nl80211_data *drv; struct i802_bss *next; @@ -193,11 +204,15 @@ struct i802_bss { int freq; struct nl80211_handles nl_preq, nl_mgmt; + + struct nl80211_wiphy_data *wiphy_data; + struct dl_list wiphy_list; }; struct wpa_driver_nl80211_data { struct nl80211_global *global; struct dl_list list; + struct dl_list wiphy_list; u8 addr[ETH_ALEN]; char phyname[32]; void *ctx; @@ -462,6 +477,224 @@ nla_put_failure: } +struct wiphy_idx_data { + int wiphy_idx; +}; + + +static int netdev_info_handler(struct nl_msg *msg, void *arg) +{ + struct nlattr *tb[NL80211_ATTR_MAX + 1]; + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct wiphy_idx_data *info = arg; + + nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), NULL); + + if (tb[NL80211_ATTR_WIPHY]) + info->wiphy_idx = nla_get_u32(tb[NL80211_ATTR_WIPHY]); + + return NL_SKIP; +} + + +static int nl80211_get_wiphy_index(struct i802_bss *bss) +{ + struct nl_msg *msg; + struct wiphy_idx_data data = { + .wiphy_idx = -1, + }; + + msg = nlmsg_alloc(); + if (!msg) + return -1; + + genlmsg_put(msg, 0, 0, genl_family_get_id(bss->drv->nl80211), 0, + 0, NL80211_CMD_GET_INTERFACE, 0); + + NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, bss->ifindex); + + if (send_and_recv_msgs(bss->drv, msg, netdev_info_handler, &data) == 0) + return data.wiphy_idx; + msg = NULL; +nla_put_failure: + nlmsg_free(msg); + return -1; +} + + +static int nl80211_register_beacons(struct wpa_driver_nl80211_data *drv, + struct nl80211_wiphy_data *w) +{ + struct nl_msg *msg; + int ret = -1; + + msg = nlmsg_alloc(); + if (!msg) + return -1; + + genlmsg_put(msg, 0, 0, genl_family_get_id(drv->nl80211), 0, 0, + NL80211_CMD_REGISTER_BEACONS, 0); + + NLA_PUT_U32(msg, NL80211_ATTR_WIPHY, w->wiphy_idx); + + ret = send_and_recv(drv, w->nl_beacons.handle, msg, NULL, NULL); + msg = NULL; + if (ret) { + wpa_printf(MSG_DEBUG, "nl80211: Register beacons command " + "failed: ret=%d (%s)", + ret, strerror(-ret)); + goto nla_put_failure; + } + ret = 0; +nla_put_failure: + nlmsg_free(msg); + return ret; +} + + +static void nl80211_recv_beacons(int sock, void *eloop_ctx, void *handle) +{ + struct nl80211_wiphy_data *w = eloop_ctx; + + wpa_printf(MSG_DEBUG, "nl80211: beacon event message available"); + + nl_recvmsgs(handle, w->nl_cb); +} + + +static int process_beacon_event(struct nl_msg *msg, void *arg) +{ + struct nl80211_wiphy_data *w = arg; + struct wpa_driver_nl80211_data *drv; + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct nlattr *tb[NL80211_ATTR_MAX + 1]; + union wpa_event_data event; + + nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), NULL); + + if (gnlh->cmd != NL80211_CMD_FRAME) { + wpa_printf(MSG_DEBUG, "unexpected event? (%d)", gnlh->cmd); + return NL_SKIP; + } + + if (!tb[NL80211_ATTR_FRAME]) + return NL_SKIP; + + dl_list_for_each(drv, &w->drvs, struct wpa_driver_nl80211_data, + wiphy_list) { + memset(&event, 0, sizeof(event)); + event.rx_mgmt.frame = nla_data(tb[NL80211_ATTR_FRAME]); + event.rx_mgmt.frame_len = nla_len(tb[NL80211_ATTR_FRAME]); + wpa_supplicant_event(drv->ctx, EVENT_RX_MGMT, &event); + } + + return NL_SKIP; +} + + +static struct nl80211_wiphy_data * +nl80211_get_wiphy_data_ap(struct i802_bss *bss) +{ + static DEFINE_DL_LIST(nl80211_wiphys); + struct nl80211_wiphy_data *w; + int wiphy_idx, found = 0; + struct i802_bss *tmp_bss; + + if (bss->wiphy_data != NULL) + return bss->wiphy_data; + + wiphy_idx = nl80211_get_wiphy_index(bss); + + dl_list_for_each(w, &nl80211_wiphys, struct nl80211_wiphy_data, list) { + if (w->wiphy_idx == wiphy_idx) + goto add; + } + + /* alloc new one */ + w = os_zalloc(sizeof(*w)); + w->wiphy_idx = wiphy_idx; + dl_list_init(&w->bsss); + dl_list_init(&w->drvs); + + w->nl_cb = nl_cb_clone(bss->drv->nl_cb); + if (!w->nl_cb) { + os_free(w); + return NULL; + } + nl_cb_set(w->nl_cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); + nl_cb_set(w->nl_cb, NL_CB_VALID, NL_CB_CUSTOM, process_beacon_event, w); + + if (nl_create_handles(&w->nl_beacons, bss->drv->nl_cb, "wiphy beacons")) { + os_free(w); + return NULL; + } + + if (nl80211_register_beacons(bss->drv, w)) { + nl_destroy_handles(&w->nl_beacons); + os_free(w); + return NULL; + } + + eloop_register_read_sock(nl_socket_get_fd(w->nl_beacons.handle), + nl80211_recv_beacons, w, + w->nl_beacons.handle); + + dl_list_add(&nl80211_wiphys, &w->list); + + add: + /* drv entry for this bss already there? */ + dl_list_for_each(tmp_bss, &w->bsss, struct i802_bss, wiphy_list) { + if (tmp_bss->drv == bss->drv) { + found = 1; + break; + } + } + /* if not add it */ + if (!found) + dl_list_add(&w->drvs, &bss->drv->wiphy_list); + + dl_list_add(&w->bsss, &bss->wiphy_list); + bss->wiphy_data = w; + return w; +} + + +static void nl80211_put_wiphy_data_ap(struct i802_bss *bss) +{ + struct nl80211_wiphy_data *w = bss->wiphy_data; + struct i802_bss *tmp_bss; + int found = 0; + + if (w == NULL) + return; + bss->wiphy_data = NULL; + dl_list_del(&bss->wiphy_list); + + /* still any for this drv present? */ + dl_list_for_each(tmp_bss, &w->bsss, struct i802_bss, wiphy_list) { + if (tmp_bss->drv == bss->drv) { + found = 1; + break; + } + } + /* if not remove it */ + if (!found) + dl_list_del(&bss->drv->wiphy_list); + + if (!dl_list_empty(&w->bsss)) + return; + + eloop_unregister_read_sock(nl_socket_get_fd(w->nl_beacons.handle)); + + nl_cb_put(w->nl_cb); + nl_destroy_handles(&w->nl_beacons); + dl_list_del(&w->list); + os_free(w); +} + + static int wpa_driver_nl80211_get_bssid(void *priv, u8 *bssid) { struct i802_bss *bss = priv; @@ -2565,6 +2798,9 @@ static int nl80211_mgmt_subscribe_ap(struct i802_bss *bss) if (nl80211_register_spurious_class3(bss)) goto out_err; + if (nl80211_get_wiphy_data_ap(bss) == NULL) + goto out_err; + return 0; out_err: @@ -2580,6 +2816,8 @@ static void nl80211_mgmt_unsubscribe(struct i802_bss *bss) return; eloop_unregister_read_sock(nl_socket_get_fd(bss->nl_mgmt.handle)); nl_destroy_handles(&bss->nl_mgmt); + + nl80211_put_wiphy_data_ap(bss); } -- 1.7.6.3