batman-adv: set skb priority according to content

The skb priority field may help the wireless driver to choose the right
queue (e.g. WMM queues). This should be set in batman-adv, as this
information is only available here.

This patch adds support for IPv4/IPv6 DS fields and VLAN PCP. Note that
only VLAN PCP is used if a VLAN header is present. Also initially set
TC_PRIO_CONTROL only for self-generated packets, and keep the priority
set by higher layers.

Signed-off-by: Simon Wunderlich <simon@open-mesh.com>
Signed-off-by: Marek Lindner <lindner_marek@yahoo.de>
Signed-off-by: Antonio Quartulli <ordex@autistici.org>

1 files changed, 58 insertions, 0 deletions

diff --git a/net/batman-adv/main.c b/net/batman-adv/main.c
index 08125f3f6064..c72d1bcdcf49 100644
--- a/net/batman-adv/main.c
+++ b/net/batman-adv/main.c
@@ -19,6 +19,10 @@
 
 #include <linux/crc32c.h>
 #include <linux/highmem.h>
+#include <linux/if_vlan.h>
+#include <net/ip.h>
+#include <net/ipv6.h>
+#include <net/dsfield.h>
 #include "main.h"
 #include "sysfs.h"
 #include "debugfs.h"
@@ -249,6 +253,60 @@ out:
         return primary_if;
 }
 
+/**
+ * batadv_skb_set_priority - sets skb priority according to packet content
+ * @skb: the packet to be sent
+ * @offset: offset to the packet content
+ *
+ * This function sets a value between 256 and 263 (802.1d priority), which
+ * can be interpreted by the cfg80211 or other drivers.
+ */
+void batadv_skb_set_priority(struct sk_buff *skb, int offset)
+{
+        struct iphdr ip_hdr_tmp, *ip_hdr;
+        struct ipv6hdr ip6_hdr_tmp, *ip6_hdr;
+        struct ethhdr ethhdr_tmp, *ethhdr;
+        struct vlan_ethhdr *vhdr, vhdr_tmp;
+        u32 prio;
+
+        /* already set, do nothing */
+        if (skb->priority >= 256 && skb->priority <= 263)
+                return;
+
+        ethhdr = skb_header_pointer(skb, offset, sizeof(*ethhdr), &ethhdr_tmp);
+        if (!ethhdr)
+                return;
+
+        switch (ethhdr->h_proto) {
+        case htons(ETH_P_8021Q):
+                vhdr = skb_header_pointer(skb, offset + sizeof(*vhdr),
+                                          sizeof(*vhdr), &vhdr_tmp);
+                if (!vhdr)
+                        return;
+                prio = ntohs(vhdr->h_vlan_TCI) & VLAN_PRIO_MASK;
+                prio = prio >> VLAN_PRIO_SHIFT;
+                break;
+        case htons(ETH_P_IP):
+                ip_hdr = skb_header_pointer(skb, offset + sizeof(*ethhdr),
+                                            sizeof(*ip_hdr), &ip_hdr_tmp);
+                if (!ip_hdr)
+                        return;
+                prio = (ipv4_get_dsfield(ip_hdr) & 0xfc) >> 5;
+                break;
+        case htons(ETH_P_IPV6):
+                ip6_hdr = skb_header_pointer(skb, offset + sizeof(*ethhdr),
+                                             sizeof(*ip6_hdr), &ip6_hdr_tmp);
+                if (!ip6_hdr)
+                        return;
+                prio = (ipv6_get_dsfield(ip6_hdr) & 0xfc) >> 5;
+                break;
+        default:
+                return;
+        }
+
+        skb->priority = prio + 256;
+}
+
 static int batadv_recv_unhandled_packet(struct sk_buff *skb,
                                         struct batadv_hard_iface *recv_if)
 {