net/core/ptp_classifier.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /* PTP classifier
0003  */
0004
0005 /* The below program is the bpf_asm (tools/net/) representation of
0006  * the opcode array in the ptp_filter structure.
0007  *
0008  * For convenience, this can easily be altered and reviewed with
0009  * bpf_asm and bpf_dbg, e.g. `./bpf_asm -c prog` where prog is a
0010  * simple file containing the below program:
0011  *
0012  * ldh [12]                        ; load ethertype
0013  *
0014  * ; PTP over UDP over IPv4 over Ethernet
0015  * test_ipv4:
0016  *   jneq #0x800, test_ipv6        ; ETH_P_IP ?
0017  *   ldb [23]                      ; load proto
0018  *   jneq #17, drop_ipv4           ; IPPROTO_UDP ?
0019  *   ldh [20]                      ; load frag offset field
0020  *   jset #0x1fff, drop_ipv4       ; don't allow fragments
0021  *   ldxb 4*([14]&0xf)             ; load IP header len
0022  *   ldh [x + 16]                  ; load UDP dst port
0023  *   jneq #319, drop_ipv4          ; is port PTP_EV_PORT ?
0024  *   ldh [x + 22]                  ; load payload
0025  *   and #0xf                      ; mask PTP_CLASS_VMASK
0026  *   or #0x10                      ; PTP_CLASS_IPV4
0027  *   ret a                         ; return PTP class
0028  *   drop_ipv4: ret #0x0           ; PTP_CLASS_NONE
0029  *
0030  * ; PTP over UDP over IPv6 over Ethernet
0031  * test_ipv6:
0032  *   jneq #0x86dd, test_8021q      ; ETH_P_IPV6 ?
0033  *   ldb [20]                      ; load proto
0034  *   jneq #17, drop_ipv6           ; IPPROTO_UDP ?
0035  *   ldh [56]                      ; load UDP dst port
0036  *   jneq #319, drop_ipv6          ; is port PTP_EV_PORT ?
0037  *   ldh [62]                      ; load payload
0038  *   and #0xf                      ; mask PTP_CLASS_VMASK
0039  *   or #0x20                      ; PTP_CLASS_IPV6
0040  *   ret a                         ; return PTP class
0041  *   drop_ipv6: ret #0x0           ; PTP_CLASS_NONE
0042  *
0043  * ; PTP over 802.1Q over Ethernet
0044  * test_8021q:
0045  *   jneq #0x8100, test_ieee1588   ; ETH_P_8021Q ?
0046  *   ldh [16]                      ; load inner type
0047  *   jneq #0x88f7, test_8021q_ipv4 ; ETH_P_1588 ?
0048  *   ldb [18]                      ; load payload
0049  *   and #0x8                      ; as we don't have ports here, test
0050  *   jneq #0x0, drop_ieee1588      ; for PTP_GEN_BIT and drop these
0051  *   ldh [18]                      ; reload payload
0052  *   and #0xf                      ; mask PTP_CLASS_VMASK
0053  *   or #0xc0                      ; PTP_CLASS_VLAN|PTP_CLASS_L2
0054  *   ret a                         ; return PTP class
0055  *
0056  * ; PTP over UDP over IPv4 over 802.1Q over Ethernet
0057  * test_8021q_ipv4:
0058  *   jneq #0x800, test_8021q_ipv6  ; ETH_P_IP ?
0059  *   ldb [27]                      ; load proto
0060  *   jneq #17, drop_8021q_ipv4     ; IPPROTO_UDP ?
0061  *   ldh [24]                      ; load frag offset field
0062  *   jset #0x1fff, drop_8021q_ipv4; don't allow fragments
0063  *   ldxb 4*([18]&0xf)             ; load IP header len
0064  *   ldh [x + 20]                  ; load UDP dst port
0065  *   jneq #319, drop_8021q_ipv4    ; is port PTP_EV_PORT ?
0066  *   ldh [x + 26]                  ; load payload
0067  *   and #0xf                      ; mask PTP_CLASS_VMASK
0068  *   or #0x90                      ; PTP_CLASS_VLAN|PTP_CLASS_IPV4
0069  *   ret a                         ; return PTP class
0070  *   drop_8021q_ipv4: ret #0x0     ; PTP_CLASS_NONE
0071  *
0072  * ; PTP over UDP over IPv6 over 802.1Q over Ethernet
0073  * test_8021q_ipv6:
0074  *   jneq #0x86dd, drop_8021q_ipv6 ; ETH_P_IPV6 ?
0075  *   ldb [24]                      ; load proto
0076  *   jneq #17, drop_8021q_ipv6           ; IPPROTO_UDP ?
0077  *   ldh [60]                      ; load UDP dst port
0078  *   jneq #319, drop_8021q_ipv6          ; is port PTP_EV_PORT ?
0079  *   ldh [66]                      ; load payload
0080  *   and #0xf                      ; mask PTP_CLASS_VMASK
0081  *   or #0xa0                      ; PTP_CLASS_VLAN|PTP_CLASS_IPV6
0082  *   ret a                         ; return PTP class
0083  *   drop_8021q_ipv6: ret #0x0     ; PTP_CLASS_NONE
0084  *
0085  * ; PTP over Ethernet
0086  * test_ieee1588:
0087  *   jneq #0x88f7, drop_ieee1588   ; ETH_P_1588 ?
0088  *   ldb [14]                      ; load payload
0089  *   and #0x8                      ; as we don't have ports here, test
0090  *   jneq #0x0, drop_ieee1588      ; for PTP_GEN_BIT and drop these
0091  *   ldh [14]                      ; reload payload
0092  *   and #0xf                      ; mask PTP_CLASS_VMASK
0093  *   or #0x40                      ; PTP_CLASS_L2
0094  *   ret a                         ; return PTP class
0095  *   drop_ieee1588: ret #0x0       ; PTP_CLASS_NONE
0096  */
0097
0098 #include <linux/skbuff.h>
0099 #include <linux/filter.h>
0100 #include <linux/ptp_classify.h>
0101
0102 static struct bpf_prog *ptp_insns __read_mostly;
0103
0104 unsigned int ptp_classify_raw(const struct sk_buff *skb)
0105 {
0106     return bpf_prog_run(ptp_insns, skb);
0107 }
0108 EXPORT_SYMBOL_GPL(ptp_classify_raw);
0109
0110 struct ptp_header *ptp_parse_header(struct sk_buff *skb, unsigned int type)
0111 {
0112     u8 *ptr = skb_mac_header(skb);
0113
0114     if (type & PTP_CLASS_VLAN)
0115         ptr += VLAN_HLEN;
0116
0117     switch (type & PTP_CLASS_PMASK) {
0118     case PTP_CLASS_IPV4:
0119         ptr += IPV4_HLEN(ptr) + UDP_HLEN;
0120         break;
0121     case PTP_CLASS_IPV6:
0122         ptr += IP6_HLEN + UDP_HLEN;
0123         break;
0124     case PTP_CLASS_L2:
0125         break;
0126     default:
0127         return NULL;
0128     }
0129
0130     ptr += ETH_HLEN;
0131
0132     /* Ensure that the entire header is present in this packet. */
0133     if (ptr + sizeof(struct ptp_header) > skb->data + skb->len)
0134         return NULL;
0135
0136     return (struct ptp_header *)ptr;
0137 }
0138 EXPORT_SYMBOL_GPL(ptp_parse_header);
0139
0140 bool ptp_msg_is_sync(struct sk_buff *skb, unsigned int type)
0141 {
0142     struct ptp_header *hdr;
0143
0144     hdr = ptp_parse_header(skb, type);
0145     if (!hdr)
0146         return false;
0147
0148     return ptp_get_msgtype(hdr, type) == PTP_MSGTYPE_SYNC;
0149 }
0150 EXPORT_SYMBOL_GPL(ptp_msg_is_sync);
0151
0152 void __init ptp_classifier_init(void)
0153 {
0154     static struct sock_filter ptp_filter[] __initdata = {
0155         { 0x28,  0,  0, 0x0000000c },
0156         { 0x15,  0, 12, 0x00000800 },
0157         { 0x30,  0,  0, 0x00000017 },
0158         { 0x15,  0,  9, 0x00000011 },
0159         { 0x28,  0,  0, 0x00000014 },
0160         { 0x45,  7,  0, 0x00001fff },
0161         { 0xb1,  0,  0, 0x0000000e },
0162         { 0x48,  0,  0, 0x00000010 },
0163         { 0x15,  0,  4, 0x0000013f },
0164         { 0x48,  0,  0, 0x00000016 },
0165         { 0x54,  0,  0, 0x0000000f },
0166         { 0x44,  0,  0, 0x00000010 },
0167         { 0x16,  0,  0, 0x00000000 },
0168         { 0x06,  0,  0, 0x00000000 },
0169         { 0x15,  0,  9, 0x000086dd },
0170         { 0x30,  0,  0, 0x00000014 },
0171         { 0x15,  0,  6, 0x00000011 },
0172         { 0x28,  0,  0, 0x00000038 },
0173         { 0x15,  0,  4, 0x0000013f },
0174         { 0x28,  0,  0, 0x0000003e },
0175         { 0x54,  0,  0, 0x0000000f },
0176         { 0x44,  0,  0, 0x00000020 },
0177         { 0x16,  0,  0, 0x00000000 },
0178         { 0x06,  0,  0, 0x00000000 },
0179         { 0x15,  0, 32, 0x00008100 },
0180         { 0x28,  0,  0, 0x00000010 },
0181         { 0x15,  0,  7, 0x000088f7 },
0182         { 0x30,  0,  0, 0x00000012 },
0183         { 0x54,  0,  0, 0x00000008 },
0184         { 0x15,  0, 35, 0x00000000 },
0185         { 0x28,  0,  0, 0x00000012 },
0186         { 0x54,  0,  0, 0x0000000f },
0187         { 0x44,  0,  0, 0x000000c0 },
0188         { 0x16,  0,  0, 0x00000000 },
0189         { 0x15,  0, 12, 0x00000800 },
0190         { 0x30,  0,  0, 0x0000001b },
0191         { 0x15,  0,  9, 0x00000011 },
0192         { 0x28,  0,  0, 0x00000018 },
0193         { 0x45,  7,  0, 0x00001fff },
0194         { 0xb1,  0,  0, 0x00000012 },
0195         { 0x48,  0,  0, 0x00000014 },
0196         { 0x15,  0,  4, 0x0000013f },
0197         { 0x48,  0,  0, 0x0000001a },
0198         { 0x54,  0,  0, 0x0000000f },
0199         { 0x44,  0,  0, 0x00000090 },
0200         { 0x16,  0,  0, 0x00000000 },
0201         { 0x06,  0,  0, 0x00000000 },
0202         { 0x15,  0,  8, 0x000086dd },
0203         { 0x30,  0,  0, 0x00000018 },
0204         { 0x15,  0,  6, 0x00000011 },
0205         { 0x28,  0,  0, 0x0000003c },
0206         { 0x15,  0,  4, 0x0000013f },
0207         { 0x28,  0,  0, 0x00000042 },
0208         { 0x54,  0,  0, 0x0000000f },
0209         { 0x44,  0,  0, 0x000000a0 },
0210         { 0x16,  0,  0, 0x00000000 },
0211         { 0x06,  0,  0, 0x00000000 },
0212         { 0x15,  0,  7, 0x000088f7 },
0213         { 0x30,  0,  0, 0x0000000e },
0214         { 0x54,  0,  0, 0x00000008 },
0215         { 0x15,  0,  4, 0x00000000 },
0216         { 0x28,  0,  0, 0x0000000e },
0217         { 0x54,  0,  0, 0x0000000f },
0218         { 0x44,  0,  0, 0x00000040 },
0219         { 0x16,  0,  0, 0x00000000 },
0220         { 0x06,  0,  0, 0x00000000 },
0221     };
0222     struct sock_fprog_kern ptp_prog;
0223
0224     ptp_prog.len = ARRAY_SIZE(ptp_filter);
0225     ptp_prog.filter = ptp_filter;
0226
0227     BUG_ON(bpf_prog_create(&ptp_insns, &ptp_prog));
0228 }