This function subtracts the ingress hardware time stamp from the
correction field of a PTP header and updates the UDP checksum (if UDP is
used as transport. It is needed for hardware capable of one-step P2P
that does not already modify the correction field of Pdelay_Req event
messages on ingress.
Signed-off-by: Christian Eggers <ceggers@xxxxxxx>
---
include/linux/ptp_classify.h | 97 ++++++++++++++++++++++++++++++++++++
1 file changed, 97 insertions(+)
diff --git a/include/linux/ptp_classify.h b/include/linux/ptp_classify.h
index 56b2d7d66177..f27b512e1abd 100644
--- a/include/linux/ptp_classify.h
+++ b/include/linux/ptp_classify.h
@@ -10,8 +10,12 @@
#ifndef _PTP_CLASSIFY_H_
#define _PTP_CLASSIFY_H_
+#include <asm/unaligned.h>
#include <linux/ip.h>
+#include <linux/ktime.h>
#include <linux/skbuff.h>
+#include <linux/udp.h>
+#include <net/checksum.h>
#define PTP_CLASS_NONE 0x00 /* not a PTP event message */
#define PTP_CLASS_V1 0x01 /* protocol version 1 */
@@ -118,6 +122,91 @@ static inline u8 ptp_get_msgtype(const struct ptp_header *hdr,
return msgtype;
}
+/**
+ * ptp_check_diff8 - Computes new checksum (when altering a 64-bit field)
+ * @old: old field value
+ * @new: new field value
+ * @oldsum: previous checksum
+ *
+ * This function can be used to calculate a new checksum when only a single
+ * field is changed. Similar as ip_vs_check_diff*() in ip_vs.h.
+ *
+ * Return: Updated checksum
+ */
+static inline __wsum ptp_check_diff8(__be64 old, __be64 new, __wsum oldsum)
+{
+ __be64 diff[2] = { ~old, new };
+
+ return csum_partial(diff, sizeof(diff), oldsum);
+}
+
+/**
+ * ptp_onestep_p2p_move_t2_to_correction - Update PTP header's correction field
+ * @skb: packet buffer
+ * @type: type of the packet (see ptp_classify_raw())
+ * @hdr: ptp header
+ * @t2: ingress hardware time stamp
+ *
+ * This function subtracts the ingress hardware time stamp from the correction
+ * field of a PTP header and updates the UDP checksum (if UDP is used as
+ * transport). It is needed for hardware capable of one-step P2P that does not
+ * already modify the correction field of Pdelay_Req event messages on ingress.
+ */
+static inline
+void ptp_onestep_p2p_move_t2_to_correction(struct sk_buff *skb,
+ unsigned int type,
+ struct ptp_header *hdr,
+ ktime_t t2)
+{
+ u8 *ptr = skb_mac_header(skb);
+ struct udphdr *uhdr = NULL;
+ s64 ns = ktime_to_ns(t2);
+ __be64 correction_old;
+ s64 correction;
+
+ /* previous correction value is required for checksum update. */
+ memcpy(&correction_old, &hdr->correction, sizeof(correction_old));
+ correction = (s64)be64_to_cpu(correction_old);
+
+ /* PTP correction field consists of 32 bit nanoseconds and 16 bit
+ * fractional nanoseconds. Avoid shifting negative numbers.
+ */
+ if (ns >= 0)
+ correction -= ns << 16;
+ else
+ correction += -ns << 16;
+
+ /* write new correction value */
+ put_unaligned_be64((u64)correction, &hdr->correction);
+
+ /* locate udp header */
+ if (type & PTP_CLASS_VLAN)
+ ptr += VLAN_HLEN;
+
+ ptr += ETH_HLEN;
+
+ switch (type & PTP_CLASS_PMASK) {
+ case PTP_CLASS_IPV4:
+ ptr += ((struct iphdr *)ptr)->ihl << 2;
+ uhdr = (struct udphdr *)ptr;
+ break;
+ case PTP_CLASS_IPV6:
+ ptr += IP6_HLEN;
+ uhdr = (struct udphdr *)ptr;
+ break;
+ }
+
+ if (!uhdr)
+ return;
+
+ /* update checksum */
+ uhdr->check = csum_fold(ptp_check_diff8(correction_old,
+ hdr->correction,
+ ~csum_unfold(uhdr->check)));
+ if (!uhdr->check)
+ uhdr->check = CSUM_MANGLED_0;
+}
+
void __init ptp_classifier_init(void);
#else
static inline void ptp_classifier_init(void)
@@ -140,5 +229,13 @@ static inline u8 ptp_get_msgtype(const struct ptp_header *hdr,
*/
return 0;
}
+
+static inline
+void ptp_onestep_p2p_move_t2_to_correction(struct sk_buff *skb,
+ unsigned int type,
+ struct ptp_header *hdr,
+ ktime_t t2)
+{
+}
#endif
#endif /* _PTP_CLASSIFY_H_ */
--
Christian Eggers
Embedded software developer
Arnold & Richter Cine Technik GmbH & Co. Betriebs KG
Sitz: Muenchen - Registergericht: Amtsgericht Muenchen - Handelsregisternummer: HRA 57918
Persoenlich haftender Gesellschafter: Arnold & Richter Cine Technik GmbH
Sitz: Muenchen - Registergericht: Amtsgericht Muenchen - Handelsregisternummer: HRB 54477
Geschaeftsfuehrer: Dr. Michael Neuhaeuser; Stephan Schenk; Walter Trauninger; Markus Zeiler
