[PATCH 5/7] wifi: brcmfmac: add the AWDL data path

[Andre Eikmeyer <dev@deq.rocks>]

The firmware passes AWDL data frames as 802.3 frames. An LLC/SNAP
header with the Apple OUI (aa aa 03 00 17 f2 08 00) is followed by the
AWDL data header 03 04, a 16 bit sequence number, two reserved bytes
and the ethertype.

Let the AWDL network device send and receive plain Ethernet frames. Its
transmit function adds the 16 bytes of headers and counts the sequence
number, brcmf_netif_rx() removes them from received frames. The headroom
of the network device covers the added header. The addresses of
received frames are moved so that they form an Ethernet header with the
ethertype again, which keeps packet taps consistent. Other frames on
the AWDL interface are dropped.

Tested-by: Yossef Osman <Yossef.Osman@proton.me>
Tested-by: Aditya Garg <aditya.garg@linux.dev>
Signed-off-by: Andre Eikmeyer <dev@deq.rocks>
---
 .../broadcom/brcm80211/brcmfmac/awdl.c        | 83 +++++++++++++++++++
 .../broadcom/brcm80211/brcmfmac/awdl.h        |  9 ++
 .../broadcom/brcm80211/brcmfmac/core.c        | 23 ++++-
 3 files changed, 114 insertions(+), 1 deletion(-)

diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/awdl.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/awdl.c
index 485ed74f3d6..95619283471 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/awdl.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/awdl.c
@@ -8,11 +8,25 @@
  * an interface of type AWDL with interface_create and configures it through
  * the awdl_* iovars. The AWDL interface has a network device but no cfg80211
  * vif. Its interface events are handled here and not by cfg80211.
+ *
+ * The firmware passes AWDL data frames as 802.3 frames with an LLC/SNAP
+ * header with the Apple OUI and an AWDL data header:
+ *
+ *   aa aa 03 00 17 f2 08 00   LLC/SNAP, OUI 00:17:f2, protocol 0x0800
+ *   03 04                     AWDL data header
+ *   seq (le16) 00 00          sequence number, reserved
+ *   ethertype (be16)
+ *
+ * The network device of the AWDL interface sends and receives plain
+ * Ethernet frames. The header is added before frames are sent and removed
+ * from received frames.
  */
 
 #include <linux/etherdevice.h>
 #include <linux/netdevice.h>
 #include <linux/rtnetlink.h>
+#include <linux/skbuff.h>
+#include <linux/unaligned.h>
 
 #include "core.h"
 #include "debug.h"
@@ -26,6 +40,14 @@
 #define BRCMF_AWDL_IFIDX		2
 #define BRCMF_AWDL_IF_TIMEOUT		msecs_to_jiffies(2000)
 
+/* LLC/SNAP with the Apple OUI and the start of the AWDL data header */
+static const u8 brcmf_awdl_llc[10] = {
+	0xaa, 0xaa, 0x03, 0x00, 0x17, 0xf2, 0x08, 0x00, 0x03, 0x04,
+};
+
+#define BRCMF_AWDL_SEQ_OFF		10
+#define BRCMF_AWDL_ETHERTYPE_OFF	14
+
 /* The BSSID of all AWDL frames */
 static const u8 brcmf_awdl_bssid[ETH_ALEN] = {
 	0x00, 0x25, 0x00, 0xff, 0x94, 0x73,
@@ -252,6 +274,67 @@ int brcmf_awdl_del(struct brcmf_pub *drvr)
 	return err;
 }
 
+/**
+ * brcmf_awdl_encap() - add the AWDL data header to a frame to be sent.
+ *
+ * @ifp: the AWDL interface.
+ * @skb: Ethernet frame, an 802.3 frame on return.
+ *
+ * Called in the transmit path of the AWDL network device, which serializes
+ * it. Return: 0 or a negative error code, the caller frees @skb on error.
+ */
+int brcmf_awdl_encap(struct brcmf_if *ifp, struct sk_buff *skb)
+{
+	struct brcmf_awdl *awdl = &ifp->drvr->awdl;
+	__be16 proto;
+	u8 *hdr;
+
+	if (skb->len < ETH_HLEN || skb_cow_head(skb, BRCMF_AWDL_HDR_LEN))
+		return -EINVAL;
+
+	proto = eth_hdr(skb)->h_proto;
+	hdr = skb_push(skb, BRCMF_AWDL_HDR_LEN);
+	memmove(hdr, hdr + BRCMF_AWDL_HDR_LEN, 2 * ETH_ALEN);
+	put_unaligned_be16(skb->len - ETH_HLEN, hdr + 2 * ETH_ALEN);
+	hdr += ETH_HLEN;
+	memcpy(hdr, brcmf_awdl_llc, sizeof(brcmf_awdl_llc));
+	put_unaligned_le16(awdl->tx_seq++, hdr + BRCMF_AWDL_SEQ_OFF);
+	put_unaligned_le16(0, hdr + BRCMF_AWDL_SEQ_OFF + 2);
+	put_unaligned(proto, (__be16 *)(hdr + BRCMF_AWDL_ETHERTYPE_OFF));
+	skb_reset_mac_header(skb);
+	return 0;
+}
+
+/**
+ * brcmf_awdl_decap() - remove the AWDL data header from a received frame.
+ *
+ * @skb: frame after eth_type_trans(), an Ethernet frame on return.
+ *
+ * The addresses are moved behind the AWDL data header, where they form an
+ * Ethernet header with the ethertype of the AWDL data header. Return: 0 or
+ * a negative error code for frames that are not AWDL data frames.
+ */
+int brcmf_awdl_decap(struct sk_buff *skb)
+{
+	__be16 proto;
+	u8 *eth;
+
+	if (skb->protocol != htons(ETH_P_802_2) ||
+	    !pskb_may_pull(skb, BRCMF_AWDL_HDR_LEN) ||
+	    memcmp(skb->data, brcmf_awdl_llc, sizeof(brcmf_awdl_llc)) ||
+	    skb_cow_head(skb, 0))
+		return -EINVAL;
+
+	proto = get_unaligned((__be16 *)(skb->data + BRCMF_AWDL_ETHERTYPE_OFF));
+	eth = skb->data + BRCMF_AWDL_HDR_LEN - ETH_HLEN;
+	memmove(eth, skb_mac_header(skb), 2 * ETH_ALEN);
+	skb_pull_rcsum(skb, BRCMF_AWDL_HDR_LEN);
+	skb_set_mac_header(skb, -ETH_HLEN);
+	skb_reset_network_header(skb);
+	skb->protocol = proto;
+	return 0;
+}
+
 /* Called before the interfaces of the device are removed */
 void brcmf_awdl_detach(struct brcmf_pub *drvr)
 {
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/awdl.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/awdl.h
index b12cbd32187..3edc0e96bb9 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/awdl.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/awdl.h
@@ -13,6 +13,8 @@
 
 struct brcmf_pub;
 struct brcmf_if;
+struct net_device;
+struct sk_buff;
 
 /**
  * struct brcmf_awdl - AWDL interface of a device.
@@ -25,6 +27,7 @@ struct brcmf_if;
  * @removing: interface_remove was sent, its interface event is awaited.
  * @detached: the device goes away, no new AWDL interface is created.
  * @ifp: the AWDL interface, NULL when there is none.
+ * @tx_seq: sequence number of the next data frame sent.
  */
 struct brcmf_awdl {
 	spinlock_t lock; /* AWDL state against the event worker */
@@ -34,8 +37,12 @@ struct brcmf_awdl {
 	bool removing;
 	bool detached;
 	struct brcmf_if *ifp;
+	u16 tx_seq;
 };
 
+/* Length of the LLC/SNAP and AWDL data header of AWDL data frames */
+#define BRCMF_AWDL_HDR_LEN		16
+
 void brcmf_awdl_attach(struct brcmf_pub *drvr);
 void brcmf_awdl_detach(struct brcmf_pub *drvr);
 int brcmf_awdl_add(struct brcmf_pub *drvr, const u8 *mac,
@@ -43,5 +50,7 @@ int brcmf_awdl_add(struct brcmf_pub *drvr, const u8 *mac,
 int brcmf_awdl_del(struct brcmf_pub *drvr);
 bool brcmf_awdl_if_added(struct brcmf_pub *drvr, struct brcmf_if *ifp);
 bool brcmf_awdl_if_removed(struct brcmf_pub *drvr, struct brcmf_if *ifp);
+int brcmf_awdl_encap(struct brcmf_if *ifp, struct sk_buff *skb);
+int brcmf_awdl_decap(struct sk_buff *skb);
 
 #endif /* BRCMFMAC_AWDL_H */
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
index 37c558b6f0d..d64d24ce65f 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
@@ -422,6 +422,12 @@ void brcmf_netif_rx(struct brcmf_if *ifp, struct sk_buff *skb)
 		return;
 	}
 
+	if (ifp->awdl && brcmf_awdl_decap(skb)) {
+		ifp->ndev->stats.rx_dropped++;
+		brcmu_pkt_buf_free_skb(skb);
+		return;
+	}
+
 	ifp->ndev->stats.rx_bytes += skb->len;
 	ifp->ndev->stats.rx_packets++;
 
@@ -653,6 +659,19 @@ static int brcmf_awdl_netdev_open(struct net_device *ndev)
 	return 0;
 }
 
+static netdev_tx_t brcmf_awdl_netdev_start_xmit(struct sk_buff *skb,
+						struct net_device *ndev)
+{
+	struct brcmf_if *ifp = netdev_priv(ndev);
+
+	if (brcmf_awdl_encap(ifp, skb)) {
+		dev_kfree_skb_any(skb);
+		ndev->stats.tx_dropped++;
+		return NETDEV_TX_OK;
+	}
+	return brcmf_netdev_start_xmit(skb, ndev);
+}
+
 static int brcmf_awdl_netdev_stop(struct net_device *ndev)
 {
 	netif_stop_queue(ndev);
@@ -667,7 +686,7 @@ static int brcmf_awdl_netdev_stop(struct net_device *ndev)
 static const struct net_device_ops brcmf_netdev_ops_awdl = {
 	.ndo_open = brcmf_awdl_netdev_open,
 	.ndo_stop = brcmf_awdl_netdev_stop,
-	.ndo_start_xmit = brcmf_netdev_start_xmit,
+	.ndo_start_xmit = brcmf_awdl_netdev_start_xmit,
 };
 
 static bool brcmf_netdev_is_brcmf(struct net_device *ndev)
@@ -693,6 +712,8 @@ int brcmf_net_attach(struct brcmf_if *ifp, bool locked)
 		ndev->netdev_ops = &brcmf_netdev_ops_pri;
 
 	ndev->needed_headroom += drvr->hdrlen;
+	if (ifp->awdl)
+		ndev->needed_headroom += BRCMF_AWDL_HDR_LEN;
 	ndev->ethtool_ops = &brcmf_ethtool_ops;
 
 	/* set the mac address & netns */