ptp: Add generic ptp v2 header parsing function

Reason: A lot of the ptp drivers - which implement hardware time stamping - need
specific fields such as the sequence id from the ptp v2 header. Currently all
drivers implement that themselves.

Introduce a generic function to retrieve a pointer to the start of the ptp v2
header.

Suggested-by: Russell King <rmk+kernel@armlinux.org.uk>
Signed-off-by: Kurt Kanzenbach <kurt@linutronix.de>
Reviewed-by: Richard Cochran <richardcochran@gmail.com>
Reviewed-by: Florian Fainelli <f.fainelli@gmail.com>
Reviewed-by: Russell King <rmk+kernel@armlinux.org.uk>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/include/linux/ptp_classify.h b/include/linux/ptp_classify.h
index dd00fa41f7e79e0f8a72e4062dfdfe364d9c554b..0a9cc0eb0801e4a05724a663a3966e5777603622 100644 (file)
--- a/include/linux/ptp_classify.h
+++ b/include/linux/ptp_classify.h
@@ -44,6 +44,30 @@
 #define OFF_IHL                14
 #define IPV4_HLEN(data) (((struct iphdr *)(data + OFF_IHL))->ihl << 2)
 
+struct clock_identity {
+       u8 id[8];
+} __packed;
+
+struct port_identity {
+       struct clock_identity   clock_identity;
+       __be16                  port_number;
+} __packed;
+
+struct ptp_header {
+       u8                      tsmt;  /* transportSpecific | messageType */
+       u8                      ver;   /* reserved          | versionPTP  */
+       __be16                  message_length;
+       u8                      domain_number;
+       u8                      reserved1;
+       u8                      flag_field[2];
+       __be64                  correction;
+       __be32                  reserved2;
+       struct port_identity    source_port_identity;
+       __be16                  sequence_id;
+       u8                      control;
+       u8                      log_message_interval;
+} __packed;
+
 #if defined(CONFIG_NET_PTP_CLASSIFY)
 /**
  * ptp_classify_raw - classify a PTP packet
@@ -57,6 +81,21 @@
  */
 unsigned int ptp_classify_raw(const struct sk_buff *skb);
 
+/**
+ * ptp_parse_header - Get pointer to the PTP v2 header
+ * @skb: packet buffer
+ * @type: type of the packet (see ptp_classify_raw())
+ *
+ * This function takes care of the VLAN, UDP, IPv4 and IPv6 headers. The length
+ * is checked.
+ *
+ * Note, internally skb_mac_header() is used. Make sure that the @skb is
+ * initialized accordingly.
+ *
+ * Return: Pointer to the ptp v2 header or NULL if not found
+ */
+struct ptp_header *ptp_parse_header(struct sk_buff *skb, unsigned int type);
+
 void __init ptp_classifier_init(void);
 #else
 static inline void ptp_classifier_init(void)
@@ -66,5 +105,10 @@ static inline unsigned int ptp_classify_raw(struct sk_buff *skb)
 {
        return PTP_CLASS_NONE;
 }
+static inline struct ptp_header *ptp_parse_header(struct sk_buff *skb,
+                                                 unsigned int type)
+{
+       return NULL;
+}
 #endif
 #endif /* _PTP_CLASSIFY_H_ */

diff --git a/net/core/ptp_classifier.c b/net/core/ptp_classifier.c
index d964a5147f22bc1b872da0381fa486c46b13ebaa..e33fde06d528bd9438799882609a50a0c1d90d6c 100644 (file)
--- a/net/core/ptp_classifier.c
+++ b/net/core/ptp_classifier.c
@@ -107,6 +107,36 @@ unsigned int ptp_classify_raw(const struct sk_buff *skb)
 }
 EXPORT_SYMBOL_GPL(ptp_classify_raw);
 
+struct ptp_header *ptp_parse_header(struct sk_buff *skb, unsigned int type)
+{
+       u8 *ptr = skb_mac_header(skb);
+
+       if (type & PTP_CLASS_VLAN)
+               ptr += VLAN_HLEN;
+
+       switch (type & PTP_CLASS_PMASK) {
+       case PTP_CLASS_IPV4:
+               ptr += IPV4_HLEN(ptr) + UDP_HLEN;
+               break;
+       case PTP_CLASS_IPV6:
+               ptr += IP6_HLEN + UDP_HLEN;
+               break;
+       case PTP_CLASS_L2:
+               break;
+       default:
+               return NULL;
+       }
+
+       ptr += ETH_HLEN;
+
+       /* Ensure that the entire header is present in this packet. */
+       if (ptr + sizeof(struct ptp_header) > skb->data + skb->len)
+               return NULL;
+
+       return (struct ptp_header *)ptr;
+}
+EXPORT_SYMBOL_GPL(ptp_parse_header);
+
 void __init ptp_classifier_init(void)
 {
        static struct sock_filter ptp_filter[] __initdata = {