174ba9207SThomas Gleixner /* SPDX-License-Identifier: GPL-2.0-or-later */
215f0127dSRichard Cochran /*
315f0127dSRichard Cochran * PTP 1588 support
415f0127dSRichard Cochran *
515f0127dSRichard Cochran * This file implements a BPF that recognizes PTP event messages.
615f0127dSRichard Cochran *
715f0127dSRichard Cochran * Copyright (C) 2010 OMICRON electronics GmbH
815f0127dSRichard Cochran */
915f0127dSRichard Cochran
1015f0127dSRichard Cochran #ifndef _PTP_CLASSIFY_H_
1115f0127dSRichard Cochran #define _PTP_CLASSIFY_H_
1215f0127dSRichard Cochran
13*2955762bSChristian Eggers #include <asm/unaligned.h>
14d94ba80eSRichard Cochran #include <linux/ip.h>
15*2955762bSChristian Eggers #include <linux/ktime.h>
16408eccceSDaniel Borkmann #include <linux/skbuff.h>
17*2955762bSChristian Eggers #include <linux/udp.h>
18*2955762bSChristian Eggers #include <net/checksum.h>
1915f0127dSRichard Cochran
2015f0127dSRichard Cochran #define PTP_CLASS_NONE 0x00 /* not a PTP event message */
2115f0127dSRichard Cochran #define PTP_CLASS_V1 0x01 /* protocol version 1 */
2215f0127dSRichard Cochran #define PTP_CLASS_V2 0x02 /* protocol version 2 */
2315f0127dSRichard Cochran #define PTP_CLASS_VMASK 0x0f /* max protocol version is 15 */
2415f0127dSRichard Cochran #define PTP_CLASS_IPV4 0x10 /* event in an IPV4 UDP packet */
2515f0127dSRichard Cochran #define PTP_CLASS_IPV6 0x20 /* event in an IPV6 UDP packet */
265f94c943SStefan Sørensen #define PTP_CLASS_L2 0x40 /* event in a L2 packet */
275f94c943SStefan Sørensen #define PTP_CLASS_PMASK 0x70 /* mask for the packet type field */
285f94c943SStefan Sørensen #define PTP_CLASS_VLAN 0x80 /* event in a VLAN tagged packet */
2915f0127dSRichard Cochran
3015f0127dSRichard Cochran #define PTP_CLASS_V1_IPV4 (PTP_CLASS_V1 | PTP_CLASS_IPV4)
3115f0127dSRichard Cochran #define PTP_CLASS_V1_IPV6 (PTP_CLASS_V1 | PTP_CLASS_IPV6) /* probably DNE */
3215f0127dSRichard Cochran #define PTP_CLASS_V2_IPV4 (PTP_CLASS_V2 | PTP_CLASS_IPV4)
3315f0127dSRichard Cochran #define PTP_CLASS_V2_IPV6 (PTP_CLASS_V2 | PTP_CLASS_IPV6)
3415f0127dSRichard Cochran #define PTP_CLASS_V2_L2 (PTP_CLASS_V2 | PTP_CLASS_L2)
3515f0127dSRichard Cochran #define PTP_CLASS_V2_VLAN (PTP_CLASS_V2 | PTP_CLASS_VLAN)
365f94c943SStefan Sørensen #define PTP_CLASS_L4 (PTP_CLASS_IPV4 | PTP_CLASS_IPV6)
3715f0127dSRichard Cochran
38076d38b8SChristian Eggers #define PTP_MSGTYPE_SYNC 0x0
39076d38b8SChristian Eggers #define PTP_MSGTYPE_DELAY_REQ 0x1
40076d38b8SChristian Eggers #define PTP_MSGTYPE_PDELAY_REQ 0x2
41076d38b8SChristian Eggers #define PTP_MSGTYPE_PDELAY_RESP 0x3
42076d38b8SChristian Eggers
4315f0127dSRichard Cochran #define PTP_EV_PORT 319
44ec15baecSVladimir Oltean #define PTP_GEN_PORT 320
45f75159e9SRichard Cochran #define PTP_GEN_BIT 0x08 /* indicates general message, if set in message type */
4615f0127dSRichard Cochran
47d94ba80eSRichard Cochran #define OFF_PTP_SOURCE_UUID 22 /* PTPv1 only */
48d94ba80eSRichard Cochran #define OFF_PTP_SEQUENCE_ID 30
49d94ba80eSRichard Cochran
505cebb40bSHarini Katakam /* PTP header flag fields */
515cebb40bSHarini Katakam #define PTP_FLAG_TWOSTEP BIT(1)
525cebb40bSHarini Katakam
53408eccceSDaniel Borkmann /* Below defines should actually be removed at some point in time. */
5415f0127dSRichard Cochran #define IP6_HLEN 40
5515f0127dSRichard Cochran #define UDP_HLEN 8
56408eccceSDaniel Borkmann #define OFF_IHL 14
57408eccceSDaniel Borkmann #define IPV4_HLEN(data) (((struct iphdr *)(data + OFF_IHL))->ihl << 2)
5815f0127dSRichard Cochran
59bdfbb63cSKurt Kanzenbach struct clock_identity {
60bdfbb63cSKurt Kanzenbach u8 id[8];
61bdfbb63cSKurt Kanzenbach } __packed;
62bdfbb63cSKurt Kanzenbach
63bdfbb63cSKurt Kanzenbach struct port_identity {
64bdfbb63cSKurt Kanzenbach struct clock_identity clock_identity;
65bdfbb63cSKurt Kanzenbach __be16 port_number;
66bdfbb63cSKurt Kanzenbach } __packed;
67bdfbb63cSKurt Kanzenbach
68bdfbb63cSKurt Kanzenbach struct ptp_header {
69bdfbb63cSKurt Kanzenbach u8 tsmt; /* transportSpecific | messageType */
70bdfbb63cSKurt Kanzenbach u8 ver; /* reserved | versionPTP */
71bdfbb63cSKurt Kanzenbach __be16 message_length;
72bdfbb63cSKurt Kanzenbach u8 domain_number;
73bdfbb63cSKurt Kanzenbach u8 reserved1;
74bdfbb63cSKurt Kanzenbach u8 flag_field[2];
75bdfbb63cSKurt Kanzenbach __be64 correction;
76bdfbb63cSKurt Kanzenbach __be32 reserved2;
77bdfbb63cSKurt Kanzenbach struct port_identity source_port_identity;
78bdfbb63cSKurt Kanzenbach __be16 sequence_id;
79bdfbb63cSKurt Kanzenbach u8 control;
80bdfbb63cSKurt Kanzenbach u8 log_message_interval;
81bdfbb63cSKurt Kanzenbach } __packed;
82bdfbb63cSKurt Kanzenbach
83408eccceSDaniel Borkmann #if defined(CONFIG_NET_PTP_CLASSIFY)
84408eccceSDaniel Borkmann /**
85408eccceSDaniel Borkmann * ptp_classify_raw - classify a PTP packet
86408eccceSDaniel Borkmann * @skb: buffer
87408eccceSDaniel Borkmann *
88408eccceSDaniel Borkmann * Runs a minimal BPF dissector to classify a network packet to
89408eccceSDaniel Borkmann * determine the PTP class. In case the skb does not contain any
90408eccceSDaniel Borkmann * PTP protocol data, PTP_CLASS_NONE will be returned, otherwise
91408eccceSDaniel Borkmann * PTP_CLASS_V1_IPV{4,6}, PTP_CLASS_V2_IPV{4,6} or
92408eccceSDaniel Borkmann * PTP_CLASS_V2_{L2,VLAN}, depending on the packet content.
93408eccceSDaniel Borkmann */
94164d8c66SDaniel Borkmann unsigned int ptp_classify_raw(const struct sk_buff *skb);
95164d8c66SDaniel Borkmann
96bdfbb63cSKurt Kanzenbach /**
97bdfbb63cSKurt Kanzenbach * ptp_parse_header - Get pointer to the PTP v2 header
98bdfbb63cSKurt Kanzenbach * @skb: packet buffer
99bdfbb63cSKurt Kanzenbach * @type: type of the packet (see ptp_classify_raw())
100bdfbb63cSKurt Kanzenbach *
101bdfbb63cSKurt Kanzenbach * This function takes care of the VLAN, UDP, IPv4 and IPv6 headers. The length
102bdfbb63cSKurt Kanzenbach * is checked.
103bdfbb63cSKurt Kanzenbach *
104bdfbb63cSKurt Kanzenbach * Note, internally skb_mac_header() is used. Make sure that the @skb is
105bdfbb63cSKurt Kanzenbach * initialized accordingly.
106bdfbb63cSKurt Kanzenbach *
107bdfbb63cSKurt Kanzenbach * Return: Pointer to the ptp v2 header or NULL if not found
108bdfbb63cSKurt Kanzenbach */
109bdfbb63cSKurt Kanzenbach struct ptp_header *ptp_parse_header(struct sk_buff *skb, unsigned int type);
110bdfbb63cSKurt Kanzenbach
111036c508bSKurt Kanzenbach /**
112036c508bSKurt Kanzenbach * ptp_get_msgtype - Extract ptp message type from given header
113036c508bSKurt Kanzenbach * @hdr: ptp header
114036c508bSKurt Kanzenbach * @type: type of the packet (see ptp_classify_raw())
115036c508bSKurt Kanzenbach *
116036c508bSKurt Kanzenbach * This function returns the message type for a given ptp header. It takes care
117036c508bSKurt Kanzenbach * of the different ptp header versions (v1 or v2).
118036c508bSKurt Kanzenbach *
119036c508bSKurt Kanzenbach * Return: The message type
120036c508bSKurt Kanzenbach */
ptp_get_msgtype(const struct ptp_header * hdr,unsigned int type)121036c508bSKurt Kanzenbach static inline u8 ptp_get_msgtype(const struct ptp_header *hdr,
122036c508bSKurt Kanzenbach unsigned int type)
123036c508bSKurt Kanzenbach {
124036c508bSKurt Kanzenbach u8 msgtype;
125036c508bSKurt Kanzenbach
126036c508bSKurt Kanzenbach if (unlikely(type & PTP_CLASS_V1)) {
127036c508bSKurt Kanzenbach /* msg type is located at the control field for ptp v1 */
128036c508bSKurt Kanzenbach msgtype = hdr->control;
129036c508bSKurt Kanzenbach } else {
130036c508bSKurt Kanzenbach msgtype = hdr->tsmt & 0x0f;
131036c508bSKurt Kanzenbach }
132036c508bSKurt Kanzenbach
133036c508bSKurt Kanzenbach return msgtype;
134036c508bSKurt Kanzenbach }
135036c508bSKurt Kanzenbach
136f72de02eSKurt Kanzenbach /**
137*2955762bSChristian Eggers * ptp_check_diff8 - Computes new checksum (when altering a 64-bit field)
138*2955762bSChristian Eggers * @old: old field value
139*2955762bSChristian Eggers * @new: new field value
140*2955762bSChristian Eggers * @oldsum: previous checksum
141*2955762bSChristian Eggers *
142*2955762bSChristian Eggers * This function can be used to calculate a new checksum when only a single
143*2955762bSChristian Eggers * field is changed. Similar as ip_vs_check_diff*() in ip_vs.h.
144*2955762bSChristian Eggers *
145*2955762bSChristian Eggers * Return: Updated checksum
146*2955762bSChristian Eggers */
ptp_check_diff8(__be64 old,__be64 new,__wsum oldsum)147*2955762bSChristian Eggers static inline __wsum ptp_check_diff8(__be64 old, __be64 new, __wsum oldsum)
148*2955762bSChristian Eggers {
149*2955762bSChristian Eggers __be64 diff[2] = { ~old, new };
150*2955762bSChristian Eggers
151*2955762bSChristian Eggers return csum_partial(diff, sizeof(diff), oldsum);
152*2955762bSChristian Eggers }
153*2955762bSChristian Eggers
154*2955762bSChristian Eggers /**
155*2955762bSChristian Eggers * ptp_header_update_correction - Update PTP header's correction field
156*2955762bSChristian Eggers * @skb: packet buffer
157*2955762bSChristian Eggers * @type: type of the packet (see ptp_classify_raw())
158*2955762bSChristian Eggers * @hdr: ptp header
159*2955762bSChristian Eggers * @correction: new correction value
160*2955762bSChristian Eggers *
161*2955762bSChristian Eggers * This updates the correction field of a PTP header and updates the UDP
162*2955762bSChristian Eggers * checksum (if UDP is used as transport). It is needed for hardware capable of
163*2955762bSChristian Eggers * one-step P2P that does not already modify the correction field of Pdelay_Req
164*2955762bSChristian Eggers * event messages on ingress.
165*2955762bSChristian Eggers */
166*2955762bSChristian Eggers static inline
ptp_header_update_correction(struct sk_buff * skb,unsigned int type,struct ptp_header * hdr,s64 correction)167*2955762bSChristian Eggers void ptp_header_update_correction(struct sk_buff *skb, unsigned int type,
168*2955762bSChristian Eggers struct ptp_header *hdr, s64 correction)
169*2955762bSChristian Eggers {
170*2955762bSChristian Eggers __be64 correction_old;
171*2955762bSChristian Eggers struct udphdr *uhdr;
172*2955762bSChristian Eggers
173*2955762bSChristian Eggers /* previous correction value is required for checksum update. */
174*2955762bSChristian Eggers memcpy(&correction_old, &hdr->correction, sizeof(correction_old));
175*2955762bSChristian Eggers
176*2955762bSChristian Eggers /* write new correction value */
177*2955762bSChristian Eggers put_unaligned_be64((u64)correction, &hdr->correction);
178*2955762bSChristian Eggers
179*2955762bSChristian Eggers switch (type & PTP_CLASS_PMASK) {
180*2955762bSChristian Eggers case PTP_CLASS_IPV4:
181*2955762bSChristian Eggers case PTP_CLASS_IPV6:
182*2955762bSChristian Eggers /* locate udp header */
183*2955762bSChristian Eggers uhdr = (struct udphdr *)((char *)hdr - sizeof(struct udphdr));
184*2955762bSChristian Eggers break;
185*2955762bSChristian Eggers default:
186*2955762bSChristian Eggers return;
187*2955762bSChristian Eggers }
188*2955762bSChristian Eggers
189*2955762bSChristian Eggers /* update checksum */
190*2955762bSChristian Eggers uhdr->check = csum_fold(ptp_check_diff8(correction_old,
191*2955762bSChristian Eggers hdr->correction,
192*2955762bSChristian Eggers ~csum_unfold(uhdr->check)));
193*2955762bSChristian Eggers if (!uhdr->check)
194*2955762bSChristian Eggers uhdr->check = CSUM_MANGLED_0;
195*2955762bSChristian Eggers
196*2955762bSChristian Eggers skb->ip_summed = CHECKSUM_NONE;
197*2955762bSChristian Eggers }
198*2955762bSChristian Eggers
199*2955762bSChristian Eggers /**
200f72de02eSKurt Kanzenbach * ptp_msg_is_sync - Evaluates whether the given skb is a PTP Sync message
201f72de02eSKurt Kanzenbach * @skb: packet buffer
202f72de02eSKurt Kanzenbach * @type: type of the packet (see ptp_classify_raw())
203f72de02eSKurt Kanzenbach *
204f72de02eSKurt Kanzenbach * This function evaluates whether the given skb is a PTP Sync message.
205f72de02eSKurt Kanzenbach *
206f72de02eSKurt Kanzenbach * Return: true if sync message, false otherwise
207f72de02eSKurt Kanzenbach */
208f72de02eSKurt Kanzenbach bool ptp_msg_is_sync(struct sk_buff *skb, unsigned int type);
209f72de02eSKurt Kanzenbach
210408eccceSDaniel Borkmann void __init ptp_classifier_init(void);
211408eccceSDaniel Borkmann #else
ptp_classifier_init(void)212408eccceSDaniel Borkmann static inline void ptp_classifier_init(void)
213408eccceSDaniel Borkmann {
214408eccceSDaniel Borkmann }
ptp_classify_raw(struct sk_buff * skb)215052fddfbSAndrew Lunn static inline unsigned int ptp_classify_raw(struct sk_buff *skb)
216052fddfbSAndrew Lunn {
217052fddfbSAndrew Lunn return PTP_CLASS_NONE;
218052fddfbSAndrew Lunn }
ptp_parse_header(struct sk_buff * skb,unsigned int type)219bdfbb63cSKurt Kanzenbach static inline struct ptp_header *ptp_parse_header(struct sk_buff *skb,
220bdfbb63cSKurt Kanzenbach unsigned int type)
221bdfbb63cSKurt Kanzenbach {
222bdfbb63cSKurt Kanzenbach return NULL;
223bdfbb63cSKurt Kanzenbach }
ptp_get_msgtype(const struct ptp_header * hdr,unsigned int type)224e6221295SYangbo Lu static inline u8 ptp_get_msgtype(const struct ptp_header *hdr,
225e6221295SYangbo Lu unsigned int type)
226e6221295SYangbo Lu {
227e6221295SYangbo Lu /* The return is meaningless. The stub function would not be
228e6221295SYangbo Lu * executed since no available header from ptp_parse_header.
229e6221295SYangbo Lu */
230076d38b8SChristian Eggers return PTP_MSGTYPE_SYNC;
231e6221295SYangbo Lu }
ptp_msg_is_sync(struct sk_buff * skb,unsigned int type)232f72de02eSKurt Kanzenbach static inline bool ptp_msg_is_sync(struct sk_buff *skb, unsigned int type)
233f72de02eSKurt Kanzenbach {
234f72de02eSKurt Kanzenbach return false;
235f72de02eSKurt Kanzenbach }
236*2955762bSChristian Eggers
237*2955762bSChristian Eggers static inline
ptp_header_update_correction(struct sk_buff * skb,unsigned int type,struct ptp_header * hdr,s64 correction)238*2955762bSChristian Eggers void ptp_header_update_correction(struct sk_buff *skb, unsigned int type,
239*2955762bSChristian Eggers struct ptp_header *hdr, s64 correction)
240*2955762bSChristian Eggers {
241*2955762bSChristian Eggers }
24215f0127dSRichard Cochran #endif
243408eccceSDaniel Borkmann #endif /* _PTP_CLASSIFY_H_ */
244