diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c index d2b423c30..da7038fea 100644 --- a/src/drivers/driver_nl80211.c +++ b/src/drivers/driver_nl80211.c @@ -278,6 +278,43 @@ static int ack_handler(struct nl_msg *msg, void *arg) return NL_STOP; } + +struct nl80211_ack_ext_arg { + int *err; + void *ext_data; +}; + + +static int ack_handler_cookie(struct nl_msg *msg, void *arg) +{ + struct nl80211_ack_ext_arg *ext_arg = arg; + struct nlattr *tb[NLMSGERR_ATTR_MAX + 1]; + u64 *cookie = ext_arg->ext_data; + struct nlattr *attrs; + size_t ack_len, attr_len; + + *ext_arg->err = 0; + ack_len = sizeof(struct nlmsghdr) + sizeof(int) + + sizeof(struct nlmsghdr); + attrs = (struct nlattr *) + ((u8 *) nlmsg_data(nlmsg_hdr(msg)) + sizeof(struct nlmsghdr) + + sizeof(int)); + if (nlmsg_hdr(msg)->nlmsg_len <= ack_len) + return NL_STOP; + + attr_len = nlmsg_hdr(msg)->nlmsg_len - ack_len; + + if(!(nlmsg_hdr(msg)->nlmsg_flags & NLM_F_ACK_TLVS)) + return NL_STOP; + + nla_parse(tb, NLMSGERR_ATTR_MAX, attrs, attr_len, NULL); + if (tb[NLMSGERR_ATTR_COOKIE]) + *cookie = nla_get_u64(tb[NLMSGERR_ATTR_COOKIE]); + + return NL_STOP; +} + + static int finish_handler(struct nl_msg *msg, void *arg) { int *ret = arg; @@ -392,7 +429,15 @@ static int send_and_recv(struct nl80211_global *global, nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); - nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); + if (ack_handler_custom) { + struct nl80211_ack_ext_arg *ext_arg = ack_data; + + ext_arg->err = &err; + nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, + ack_handler_custom, ack_data); + } else { + nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); + } if (valid_handler) nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, @@ -5292,8 +5337,10 @@ static int nl80211_tx_control_port(void *priv, const u8 *dest, u16 proto, const u8 *buf, size_t len, int no_encrypt) { + struct nl80211_ack_ext_arg ext_arg; struct i802_bss *bss = priv; struct nl_msg *msg; + u64 cookie = 0; int ret; wpa_printf(MSG_DEBUG, @@ -5312,11 +5359,18 @@ static int nl80211_tx_control_port(void *priv, const u8 *dest, return -ENOBUFS; } - ret = send_and_recv_msgs(bss->drv, msg, NULL, NULL, NULL, NULL); + os_memset(&ext_arg, 0, sizeof(struct nl80211_ack_ext_arg)); + ext_arg.ext_data = &cookie; + ret = send_and_recv_msgs(bss->drv, msg, NULL, NULL, + ack_handler_cookie, &ext_arg); if (ret) wpa_printf(MSG_DEBUG, "nl80211: tx_control_port failed: ret=%d (%s)", ret, strerror(-ret)); + else + wpa_printf(MSG_DEBUG, + "nl80211: tx_control_port cookie=0x%llx", + (long long unsigned int) cookie); return ret; }