From 8646ab5bddb734643236786bd8f2c9b08ac18cb3 Mon Sep 17 00:00:00 2001
From: Andrew Zaborowski <andrew.zaborowski@intel.com>
Date: Fri, 23 Dec 2016 05:38:00 -0500
Subject: [PATCH] netdev: Handle Action Frames in netdev

Action Frames are sent by nl80211 as unicast data.  We're not receiving
any other unicast packets in iwd at this time so let netdev directly
handle all unicast data on the genl socket.
---
 src/netdev.c | 116 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 116 insertions(+)

diff --git a/src/netdev.c b/src/netdev.c
index ea2d7c47..35904d04 100644
--- a/src/netdev.c
+++ b/src/netdev.c
@@ -1489,6 +1489,75 @@ int netdev_disconnect(struct netdev *netdev,
 	return 0;
 }
 
+static void netdev_action_frame_event(struct netdev *netdev,
+					const uint8_t *data, size_t len)
+{
+	uint8_t category;
+
+	if (len < 1) {
+		l_debug("Action frame too short");
+		return;
+	}
+
+	category = data[0];
+
+	switch (category) {
+	default:
+		l_debug("Unknown action frame category %u received", category);
+		break;
+	}
+}
+
+static void netdev_mgmt_frame_event(struct l_genl_msg *msg,
+					struct netdev *netdev)
+{
+	struct l_genl_attr attr;
+	uint16_t type, len, body_len = 0;
+	const void *data, *body = NULL;
+	uint16_t frame_type;
+
+	if (!l_genl_attr_init(&attr, msg))
+		return;
+
+	while (l_genl_attr_next(&attr, &type, &len, &data)) {
+		switch (type) {
+		case NL80211_ATTR_FRAME:
+			if (body)
+				return;
+
+			body = data;
+			body_len = len;
+			break;
+		}
+	}
+
+	if (!body || body_len < 25)
+		return;
+
+	frame_type = l_get_le16(body + 0);
+
+	if (memcmp(body + 4, netdev->addr, 6))
+		return;
+
+	/* Is this a management frame */
+	if (((frame_type >> 2) & 3) != 0) {
+		l_debug("Unknown frame of type %04x received",
+				(unsigned) frame_type);
+		return;
+	}
+
+	switch ((frame_type >> 4) & 15) {
+	case 0xd: /* Action frame */
+		netdev_action_frame_event(netdev, body + 24, body_len - 24);
+		break;
+
+	default:
+		l_debug("Unknown frame of type %04x received",
+				(unsigned) frame_type);
+		break;
+	}
+}
+
 static void netdev_mlme_notify(struct l_genl_msg *msg, void *user_data)
 {
 	struct netdev *netdev = NULL;
@@ -1547,6 +1616,48 @@ static void netdev_mlme_notify(struct l_genl_msg *msg, void *user_data)
 	}
 }
 
+static void netdev_unicast_notify(struct l_genl_msg *msg, void *user_data)
+{
+	struct netdev *netdev = NULL;
+	struct l_genl_attr attr;
+	uint16_t type, len;
+	const void *data;
+	uint8_t cmd;
+
+	cmd = l_genl_msg_get_command(msg);
+	if (!cmd)
+		return;
+
+	l_debug("Unicast notification %u", cmd);
+
+	if (!l_genl_attr_init(&attr, msg))
+		return;
+
+	while (l_genl_attr_next(&attr, &type, &len, &data)) {
+		switch (type) {
+		case NL80211_ATTR_IFINDEX:
+			if (len != sizeof(uint32_t)) {
+				l_warn("Invalid interface index attribute");
+				return;
+			}
+
+			netdev = netdev_find(*((uint32_t *) data));
+			break;
+		}
+	}
+
+	if (!netdev) {
+		l_warn("Unicast notification is missing ifindex attribute");
+		return;
+	}
+
+	switch (cmd) {
+	case NL80211_CMD_FRAME:
+		netdev_mgmt_frame_event(msg, netdev);
+		break;
+	}
+}
+
 struct netdev_watch_event_data {
 	struct netdev *netdev;
 	enum netdev_watch_event type;
@@ -1970,6 +2081,7 @@ bool netdev_init(struct l_genl_family *in,
 				const char *whitelist, const char *blacklist)
 {
 	struct l_genl_msg *msg;
+	struct l_genl *genl = l_genl_family_get_genl(in);
 
 	if (rtnl)
 		return false;
@@ -2009,6 +2121,10 @@ bool netdev_init(struct l_genl_family *in,
 								NULL, NULL))
 		l_error("Registering for MLME notification failed");
 
+	if (!l_genl_set_unicast_handler(genl, netdev_unicast_notify,
+								NULL, NULL))
+		l_error("Registering for unicast notification failed");
+
 	__handshake_set_install_tk_func(netdev_set_tk);
 	__handshake_set_install_gtk_func(netdev_set_gtk);
 	__handshake_set_install_igtk_func(netdev_set_igtk);