ieee802154: add ieee802154_skb_src_pan helper

This patch adds ieee802154_skb_src_pan function to get the pointer
address of the source pan id at skb mac pointer.

Signed-off-by: Alexander Aring <aar@pengutronix.de>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

diff --git a/include/linux/ieee802154.h b/include/linux/ieee802154.h
index 91f4665fea6364d5d4f80a3109d45f1f74b621ea..ddb890174a0e92e8d897bc61cc51e11cbd276067 100644 (file)
--- a/include/linux/ieee802154.h
+++ b/include/linux/ieee802154.h
@@ -50,6 +50,7 @@
 
 #define IEEE802154_EXTENDED_ADDR_LEN   8
 #define IEEE802154_SHORT_ADDR_LEN      2
+#define IEEE802154_PAN_ID_LEN          2
 
 #define IEEE802154_LIFS_PERIOD         40
 #define IEEE802154_SIFS_PERIOD         12
@@ -224,12 +225,15 @@ enum {
 #define IEEE802154_FCTL_SECEN          0x0004
 #define IEEE802154_FCTL_INTRA_PAN      0x0040
 #define IEEE802154_FCTL_DADDR          0x0c00
+#define IEEE802154_FCTL_SADDR          0xc000
 
 #define IEEE802154_FTYPE_DATA          0x0001
 
 #define IEEE802154_FCTL_ADDR_NONE      0x0000
 #define IEEE802154_FCTL_DADDR_SHORT    0x0800
 #define IEEE802154_FCTL_DADDR_EXTENDED 0x0c00
+#define IEEE802154_FCTL_SADDR_SHORT    0x8000
+#define IEEE802154_FCTL_SADDR_EXTENDED 0xc000
 
 /*
  * ieee802154_is_data - check if type is IEEE802154_FTYPE_DATA
@@ -277,6 +281,15 @@ static inline __le16 ieee802154_daddr_mode(__le16 fc)
        return fc & cpu_to_le16(IEEE802154_FCTL_DADDR);
 }
 
+/*
+ * ieee802154_saddr_mode - get saddr mode from fc
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline __le16 ieee802154_saddr_mode(__le16 fc)
+{
+       return fc & cpu_to_le16(IEEE802154_FCTL_SADDR);
+}
+
 /**
  * ieee802154_is_valid_psdu_len - check if psdu len is valid
  * available lengths:

diff --git a/include/net/mac802154.h b/include/net/mac802154.h
index b3f7cd868fe917f62c013f1de63361e7a95ff4b3..ec01b35bc969e46c0fea311e8857884907175ab8 100644 (file)
--- a/include/net/mac802154.h
+++ b/include/net/mac802154.h
@@ -286,6 +286,65 @@ static inline unsigned char *ieee802154_skb_dst_pan(__le16 fc,
        return dst_pan;
 }
 
+/**
+ * ieee802154_skb_src_pan - get the pointer to source pan field
+ * @fc: mac header frame control field
+ * @skb: skb where the source pan pointer will be get from
+ */
+static inline unsigned char *ieee802154_skb_src_pan(__le16 fc,
+                                                   const struct sk_buff *skb)
+{
+       unsigned char *src_pan;
+
+       switch (ieee802154_saddr_mode(fc)) {
+       case cpu_to_le16(IEEE802154_FCTL_ADDR_NONE):
+               src_pan = NULL;
+               break;
+       case cpu_to_le16(IEEE802154_FCTL_SADDR_SHORT):
+       case cpu_to_le16(IEEE802154_FCTL_SADDR_EXTENDED):
+               /* if intra-pan and source addr mode is non none,
+                * then source pan id is equal destination pan id.
+                */
+               if (ieee802154_is_intra_pan(fc)) {
+                       src_pan = ieee802154_skb_dst_pan(fc, skb);
+                       break;
+               }
+
+               switch (ieee802154_daddr_mode(fc)) {
+               case cpu_to_le16(IEEE802154_FCTL_ADDR_NONE):
+                       src_pan = skb_mac_header(skb) +
+                                 IEEE802154_FC_LEN +
+                                 IEEE802154_SEQ_LEN;
+                       break;
+               case cpu_to_le16(IEEE802154_FCTL_DADDR_SHORT):
+                       src_pan = skb_mac_header(skb) +
+                                 IEEE802154_FC_LEN +
+                                 IEEE802154_SEQ_LEN +
+                                 IEEE802154_PAN_ID_LEN +
+                                 IEEE802154_SHORT_ADDR_LEN;
+                       break;
+               case cpu_to_le16(IEEE802154_FCTL_DADDR_EXTENDED):
+                       src_pan = skb_mac_header(skb) +
+                                 IEEE802154_FC_LEN +
+                                 IEEE802154_SEQ_LEN +
+                                 IEEE802154_PAN_ID_LEN +
+                                 IEEE802154_EXTENDED_ADDR_LEN;
+                       break;
+               default:
+                       WARN_ONCE(1, "invalid addr mode detected");
+                       src_pan = NULL;
+                       break;
+               }
+               break;
+       default:
+               WARN_ONCE(1, "invalid addr mode detected");
+               src_pan = NULL;
+               break;
+       }
+
+       return src_pan;
+}
+
 /**
  * ieee802154_be64_to_le64 - copies and convert be64 to le64
  * @le64_dst: le64 destination pointer