nl80211: Add support for off-channel Action TX/RX commands

The kernel side support for this was just added into
wireless-testing.git. This commit adds the driver wrapper code needed
to allow wpa_supplicant to use the new functionality.

src/drivers/driver_nl80211.c

@@ -4,7 +4,7 @@
  * Copyright (c) 2003-2004, Instant802 Networks, Inc.
  * Copyright (c) 2005-2006, Devicescape Software, Inc.
  * Copyright (c) 2007, Johannes Berg <johannes@sipsolutions.net>
- * Copyright (c) 2009, Atheros Communications
+ * Copyright (c) 2009-2010, Atheros Communications
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -101,10 +101,12 @@ struct wpa_driver_nl80211_data {
 
 	unsigned int beacon_set:1;
 	unsigned int pending_remain_on_chan:1;
+	unsigned int pending_send_action:1;
 	unsigned int added_bridge:1;
 	unsigned int added_if_into_bridge:1;
 
 	u64 remain_on_chan_cookie;
+	u64 send_action_cookie;
 
 #ifdef HOSTAPD
 	int eapol_sock; /* socket for EAPOL frames */
@@ -178,10 +180,10 @@ static int no_seq_check(struct nl_msg *msg, void *arg)
 }
 
 
-static int send_and_recv_msgs(struct wpa_driver_nl80211_data *drv,
+static int send_and_recv(struct wpa_driver_nl80211_data *drv,
-			      struct nl_msg *msg,
+			 struct nl_handle *nl_handle, struct nl_msg *msg,
-			      int (*valid_handler)(struct nl_msg *, void *),
+			 int (*valid_handler)(struct nl_msg *, void *),
-			      void *valid_data)
+			 void *valid_data)
 {
 	struct nl_cb *cb;
 	int err = -ENOMEM;
@@ -190,7 +192,7 @@ static int send_and_recv_msgs(struct wpa_driver_nl80211_data *drv,
 	if (!cb)
 		goto out;
 
-	err = nl_send_auto_complete(drv->nl_handle, msg);
+	err = nl_send_auto_complete(nl_handle, msg);
 	if (err < 0)
 		goto out;
 
@@ -205,7 +207,7 @@ static int send_and_recv_msgs(struct wpa_driver_nl80211_data *drv,
 			  valid_handler, valid_data);
 
 	while (err > 0)
-		nl_recvmsgs(drv->nl_handle, cb);
+		nl_recvmsgs(nl_handle, cb);
  out:
 	nl_cb_put(cb);
 	nlmsg_free(msg);
@@ -213,6 +215,16 @@ static int send_and_recv_msgs(struct wpa_driver_nl80211_data *drv,
 }
 
 
+static int send_and_recv_msgs(struct wpa_driver_nl80211_data *drv,
+			      struct nl_msg *msg,
+			      int (*valid_handler)(struct nl_msg *, void *),
+			      void *valid_data)
+{
+	return send_and_recv(drv, drv->nl_handle, msg, valid_handler,
+			     valid_data);
+}
+
+
 struct family_data {
 	const char *group;
 	int id;
@@ -581,9 +593,74 @@ static void mlme_timeout_event(struct wpa_driver_nl80211_data *drv,
 }
 
 
+static void mlme_event_action(struct wpa_driver_nl80211_data *drv,
+			      struct nlattr *freq, const u8 *frame, size_t len)
+{
+	const struct ieee80211_mgmt *mgmt;
+	union wpa_event_data event;
+	u16 fc, stype;
+
+	mgmt = (const struct ieee80211_mgmt *) frame;
+	if (len < 24) {
+		wpa_printf(MSG_DEBUG, "nl80211: Too short action frame");
+		return;
+	}
+
+	fc = le_to_host16(mgmt->frame_control);
+	stype = WLAN_FC_GET_STYPE(fc);
+
+	os_memset(&event, 0, sizeof(event));
+	event.rx_action.da = mgmt->da;
+	event.rx_action.sa = mgmt->sa;
+	event.rx_action.bssid = mgmt->bssid;
+	event.rx_action.category = mgmt->u.action.category;
+	event.rx_action.data = &mgmt->u.action.category + 1;
+	event.rx_action.len = frame + len - event.rx_action.data;
+	if (freq)
+		event.rx_action.freq = nla_get_u32(freq);
+	wpa_supplicant_event(drv->ctx, EVENT_RX_ACTION, &event);
+}
+
+
+static void mlme_event_action_tx_status(struct wpa_driver_nl80211_data *drv,
+					struct nlattr *cookie, const u8 *frame,
+					size_t len, struct nlattr *ack)
+{
+	union wpa_event_data event;
+	const struct ieee80211_hdr *hdr;
+	u16 fc;
+	u64 cookie_val;
+
+	if (!cookie)
+		return;
+
+	cookie_val = nla_get_u64(cookie);
+	wpa_printf(MSG_DEBUG, "nl80211: Action TX status: cookie=0%llx%s",
+		   (long long unsigned int) cookie_val,
+		   cookie_val == drv->send_action_cookie ?
+		   " (match)" : " (unknown)");
+	if (cookie_val != drv->send_action_cookie)
+		return;
+
+	hdr = (const struct ieee80211_hdr *) frame;
+	fc = le_to_host16(hdr->frame_control);
+
+	os_memset(&event, 0, sizeof(event));
+	event.tx_status.type = WLAN_FC_GET_TYPE(fc);
+	event.tx_status.stype = WLAN_FC_GET_STYPE(fc);
+	event.tx_status.dst = hdr->addr1;
+	event.tx_status.data = frame;
+	event.tx_status.data_len = len;
+	event.tx_status.ack = ack != NULL;
+	wpa_supplicant_event(drv->ctx, EVENT_TX_STATUS, &event);
+}
+
+
 static void mlme_event(struct wpa_driver_nl80211_data *drv,
 		       enum nl80211_commands cmd, struct nlattr *frame,
-		       struct nlattr *addr, struct nlattr *timed_out)
+		       struct nlattr *addr, struct nlattr *timed_out,
+		       struct nlattr *freq, struct nlattr *ack,
+		       struct nlattr *cookie)
 {
 	if (timed_out && addr) {
 		mlme_timeout_event(drv, cmd, addr);
@@ -615,6 +692,13 @@ static void mlme_event(struct wpa_driver_nl80211_data *drv,
 		drv->associated = 0;
 		wpa_supplicant_event(drv->ctx, EVENT_DISASSOC, NULL);
 		break;
+	case NL80211_CMD_ACTION:
+		mlme_event_action(drv, freq, nla_data(frame), nla_len(frame));
+		break;
+	case NL80211_CMD_ACTION_TX_STATUS:
+		mlme_event_action_tx_status(drv, cookie, nla_data(frame),
+					    nla_len(frame), ack);
+		break;
 	default:
 		break;
 	}
@@ -813,8 +897,12 @@ static int process_event(struct nl_msg *msg, void *arg)
 	case NL80211_CMD_ASSOCIATE:
 	case NL80211_CMD_DEAUTHENTICATE:
 	case NL80211_CMD_DISASSOCIATE:
+	case NL80211_CMD_ACTION:
+	case NL80211_CMD_ACTION_TX_STATUS:
 		mlme_event(drv, gnlh->cmd, tb[NL80211_ATTR_FRAME],
-			   tb[NL80211_ATTR_MAC], tb[NL80211_ATTR_TIMED_OUT]);
+			   tb[NL80211_ATTR_MAC], tb[NL80211_ATTR_TIMED_OUT],
+			   tb[NL80211_ATTR_WIPHY_FREQ], tb[NL80211_ATTR_ACK],
+			   tb[NL80211_ATTR_COOKIE]);
 		break;
 	case NL80211_CMD_CONNECT:
 	case NL80211_CMD_ROAM:
@@ -1203,6 +1291,48 @@ failed:
 }
 
 
+static int nl80211_register_action_frame(struct wpa_driver_nl80211_data *drv,
+					 const u8 *match, size_t match_len)
+{
+	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_ACTION, 0);
+
+	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, drv->ifindex);
+	NLA_PUT(msg, NL80211_ATTR_FRAME_MATCH, match_len, match);
+
+	ret = send_and_recv(drv, drv->nl_handle_event, msg, NULL, NULL);
+	msg = NULL;
+	if (ret) {
+		wpa_printf(MSG_DEBUG, "nl80211: Register Action command "
+			   "failed: ret=%d (%s)", ret, strerror(-ret));
+		wpa_hexdump(MSG_DEBUG, "nl80211: Register Action match",
+			    match, match_len);
+		goto nla_put_failure;
+	}
+	ret = 0;
+nla_put_failure:
+	nlmsg_free(msg);
+	return ret;
+}
+
+
+static int nl80211_register_action_frames(struct wpa_driver_nl80211_data *drv)
+{
+	if (0) {
+		/* Public Action frames */
+		return nl80211_register_action_frame(drv, (u8 *) "\x04", 1);
+	}
+	return 0;
+}
+
+
 static int
 wpa_driver_nl80211_finish_drv_init(struct wpa_driver_nl80211_data *drv)
 {
@@ -1227,6 +1357,9 @@ wpa_driver_nl80211_finish_drv_init(struct wpa_driver_nl80211_data *drv)
 			       1, IF_OPER_DORMANT);
 #endif /* HOSTAPD */
 
+	if (nl80211_register_action_frames(drv) < 0)
+		return -1;
+
 	return 0;
 }
 
@@ -4578,6 +4711,70 @@ static int cookie_handler(struct nl_msg *msg, void *arg)
 }
 
 
+static int wpa_driver_nl80211_send_action(void *priv, unsigned int freq,
+					  const u8 *dst, const u8 *src,
+					  const u8 *bssid,
+					  const u8 *data, size_t data_len)
+{
+	struct wpa_driver_nl80211_data *drv = priv;
+	int ret = -1;
+	struct nl_msg *msg;
+	u8 *buf;
+	struct ieee80211_hdr *hdr;
+	u64 cookie;
+
+	wpa_printf(MSG_DEBUG, "nl80211: Send Action frame (ifindex=%d)",
+		   drv->ifindex);
+
+	buf = os_zalloc(24 + data_len);
+	if (buf == NULL)
+		return ret;
+	os_memcpy(buf + 24, data, data_len);
+	hdr = (struct ieee80211_hdr *) buf;
+	hdr->frame_control =
+		IEEE80211_FC(WLAN_FC_TYPE_MGMT, WLAN_FC_STYPE_ACTION);
+	os_memcpy(hdr->addr1, dst, ETH_ALEN);
+	os_memcpy(hdr->addr2, src, ETH_ALEN);
+	os_memcpy(hdr->addr3, bssid, ETH_ALEN);
+
+	if (drv->nlmode == NL80211_IFTYPE_AP) {
+		ret = wpa_driver_nl80211_send_mlme(priv, buf, 24 + data_len);
+		os_free(buf);
+		return ret;
+	}
+
+	msg = nlmsg_alloc();
+	if (!msg)
+		return -1;
+
+	genlmsg_put(msg, 0, 0, genl_family_get_id(drv->nl80211), 0, 0,
+		    NL80211_CMD_ACTION, 0);
+
+	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, drv->ifindex);
+	NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, freq);
+	NLA_PUT(msg, NL80211_ATTR_FRAME, 24 + data_len, buf);
+	os_free(buf);
+
+	cookie = 0;
+	ret = send_and_recv_msgs(drv, msg, cookie_handler, &cookie);
+	msg = NULL;
+	if (ret) {
+		wpa_printf(MSG_DEBUG, "nl80211: Action command failed: ret=%d "
+			   "(%s)", ret, strerror(-ret));
+		goto nla_put_failure;
+	}
+	wpa_printf(MSG_DEBUG, "nl80211: Action TX command accepted; "
+		   "cookie 0x%llx", (long long unsigned int) cookie);
+	drv->send_action_cookie = cookie;
+	drv->pending_send_action = 1;
+	ret = 0;
+
+nla_put_failure:
+	nlmsg_free(msg);
+	return ret;
+}
+
+
 static int wpa_driver_nl80211_remain_on_channel(void *priv, unsigned int freq,
 						unsigned int duration)
 {
@@ -4847,6 +5044,7 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.set_sta_vlan = i802_set_sta_vlan,
 	.set_wds_sta = i802_set_wds_sta,
 #endif /* HOSTAPD */
+	.send_action = wpa_driver_nl80211_send_action,
 	.remain_on_channel = wpa_driver_nl80211_remain_on_channel,
 	.cancel_remain_on_channel =
 	wpa_driver_nl80211_cancel_remain_on_channel,