From 5d0f1bbb31b4dd34e93810312fe6e4cfa6c28e35 Mon Sep 17 00:00:00 2001
From: Denis Kenzior <denkenz@gmail.com>
Date: Tue, 1 May 2018 13:31:20 -0500
Subject: [PATCH] netdev: Handle Control Port TX path

---
 src/netdev.c | 108 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 108 insertions(+)

diff --git a/src/netdev.c b/src/netdev.c
index 0d3bfd4f..36d5d35e 100644
--- a/src/netdev.c
+++ b/src/netdev.c
@@ -2145,6 +2145,12 @@ static struct l_genl_msg *netdev_build_cmd_connect(struct netdev *netdev,
 						bss->ssid_len, bss->ssid);
 	l_genl_msg_append_attr(msg, NL80211_ATTR_AUTH_TYPE, 4, &auth_type);
 
+	if (wiphy_get_ext_feature(netdev->wiphy,
+				NL80211_EXT_FEATURE_CONTROL_PORT_OVER_NL80211))
+		l_genl_msg_append_attr(msg,
+				NL80211_ATTR_CONTROL_PORT_OVER_NL80211,
+				0, NULL);
+
 	if (prev_bssid)
 		l_genl_msg_append_attr(msg, NL80211_ATTR_PREV_BSSID, ETH_ALEN,
 						prev_bssid);
@@ -3169,6 +3175,107 @@ static void netdev_control_port_frame_event(struct l_genl_msg *msg,
 						frame, frame_len, unencrypted);
 }
 
+static struct l_genl_msg *netdev_build_control_port_frame(struct netdev *netdev,
+							const uint8_t *to,
+							uint16_t proto,
+							bool unencrypted,
+							const void *body,
+							size_t body_len)
+{
+	struct l_genl_msg *msg;
+
+	msg = l_genl_msg_new_sized(NL80211_CMD_CONTROL_PORT_FRAME,
+							128 + body_len);
+
+	l_genl_msg_append_attr(msg, NL80211_ATTR_IFINDEX, 4, &netdev->index);
+	l_genl_msg_append_attr(msg, NL80211_ATTR_FRAME, body_len, body);
+	l_genl_msg_append_attr(msg, NL80211_ATTR_CONTROL_PORT_ETHERTYPE, 2,
+				&proto);
+	l_genl_msg_append_attr(msg, NL80211_ATTR_MAC, ETH_ALEN, to);
+
+	if (unencrypted)
+		l_genl_msg_append_attr(msg,
+				NL80211_ATTR_CONTROL_PORT_NO_ENCRYPT, 0, NULL);
+
+	return msg;
+}
+
+static void netdev_control_port_frame_cb(struct l_genl_msg *msg,
+							void *user_data)
+{
+	int err;
+
+	err = l_genl_msg_get_error(msg);
+
+	l_debug("%d", err);
+
+	if (err < 0)
+		l_info("CMD_CONTROL_PORT failed: %s", strerror(-err));
+}
+
+static int netdev_control_port_write_pae(struct netdev *netdev,
+						const uint8_t *dest,
+						uint16_t proto,
+						const struct eapol_frame *ef,
+						bool noencrypt)
+{
+	int fd = l_io_get_fd(netdev->pae_io);
+	struct sockaddr_ll sll;
+	size_t frame_size = sizeof(struct eapol_header) +
+					L_BE16_TO_CPU(ef->header.packet_len);
+	ssize_t r;
+
+	memset(&sll, 0, sizeof(sll));
+	sll.sll_family = AF_PACKET;
+	sll.sll_ifindex = netdev->index;
+	sll.sll_protocol = htons(proto);
+	sll.sll_halen = ETH_ALEN;
+	memcpy(sll.sll_addr, dest, ETH_ALEN);
+
+	r = sendto(fd, ef, frame_size, 0,
+			(struct sockaddr *) &sll, sizeof(sll));
+	if (r < 0)
+		l_error("EAPoL write socket: %s", strerror(errno));
+
+	return r;
+}
+
+static int netdev_control_port_frame(uint32_t ifindex,
+					const uint8_t *dest, uint16_t proto,
+					const struct eapol_frame *ef,
+					bool noencrypt,
+					void *user_data)
+{
+	struct l_genl_msg *msg;
+	struct netdev *netdev;
+	size_t frame_size;
+
+	netdev = netdev_find(ifindex);
+	if (!netdev)
+		return -ENOENT;
+
+	frame_size = sizeof(struct eapol_header) +
+			L_BE16_TO_CPU(ef->header.packet_len);
+
+	if (!wiphy_get_ext_feature(netdev->wiphy,
+			NL80211_EXT_FEATURE_CONTROL_PORT_OVER_NL80211))
+		return netdev_control_port_write_pae(netdev, dest, proto,
+							ef, noencrypt);
+
+	msg = netdev_build_control_port_frame(netdev, dest, proto, noencrypt,
+						ef, frame_size);
+	if (!msg)
+		return -ENOMEM;
+
+	if (!l_genl_family_send(nl80211, msg, netdev_control_port_frame_cb,
+				netdev, NULL)) {
+		l_genl_msg_unref(msg);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static void netdev_unicast_notify(struct l_genl_msg *msg, void *user_data)
 {
 	struct netdev *netdev = NULL;
@@ -3938,6 +4045,7 @@ bool netdev_init(struct l_genl_family *in,
 
 	__eapol_set_deauthenticate_func(netdev_handshake_failed);
 	__eapol_set_rekey_offload_func(netdev_set_rekey_offload);
+	__eapol_set_tx_packet_func(netdev_control_port_frame);
 
 	if (whitelist)
 		whitelist_filter = l_strsplit(whitelist, ',');