1// SPDX-License-Identifier: GPL-2.0-only 2/* PTP classifier 3 */ 4 5/* The below program is the bpf_asm (tools/net/) representation of 6 * the opcode array in the ptp_filter structure. 7 * 8 * For convenience, this can easily be altered and reviewed with 9 * bpf_asm and bpf_dbg, e.g. `./bpf_asm -c prog` where prog is a 10 * simple file containing the below program: 11 * 12 * ldh [12] ; load ethertype 13 * 14 * ; PTP over UDP over IPv4 over Ethernet 15 * test_ipv4: 16 * jneq #0x800, test_ipv6 ; ETH_P_IP ? 17 * ldb [23] ; load proto 18 * jneq #17, drop_ipv4 ; IPPROTO_UDP ? 19 * ldh [20] ; load frag offset field 20 * jset #0x1fff, drop_ipv4 ; don't allow fragments 21 * ldxb 4*([14]&0xf) ; load IP header len 22 * ldh [x + 16] ; load UDP dst port 23 * jneq #319, drop_ipv4 ; is port PTP_EV_PORT ? 24 * ldh [x + 22] ; load payload 25 * and #0xf ; mask PTP_CLASS_VMASK 26 * or #0x10 ; PTP_CLASS_IPV4 27 * ret a ; return PTP class 28 * drop_ipv4: ret #0x0 ; PTP_CLASS_NONE 29 * 30 * ; PTP over UDP over IPv6 over Ethernet 31 * test_ipv6: 32 * jneq #0x86dd, test_8021q ; ETH_P_IPV6 ? 33 * ldb [20] ; load proto 34 * jneq #17, drop_ipv6 ; IPPROTO_UDP ? 35 * ldh [56] ; load UDP dst port 36 * jneq #319, drop_ipv6 ; is port PTP_EV_PORT ? 37 * ldh [62] ; load payload 38 * and #0xf ; mask PTP_CLASS_VMASK 39 * or #0x20 ; PTP_CLASS_IPV6 40 * ret a ; return PTP class 41 * drop_ipv6: ret #0x0 ; PTP_CLASS_NONE 42 * 43 * ; PTP over 802.1Q over Ethernet 44 * test_8021q: 45 * jneq #0x8100, test_ieee1588 ; ETH_P_8021Q ? 46 * ldh [16] ; load inner type 47 * jneq #0x88f7, test_8021q_ipv4 ; ETH_P_1588 ? 48 * ldb [18] ; load payload 49 * and #0x8 ; as we don't have ports here, test 50 * jneq #0x0, drop_ieee1588 ; for PTP_GEN_BIT and drop these 51 * ldh [18] ; reload payload 52 * and #0xf ; mask PTP_CLASS_VMASK 53 * or #0xc0 ; PTP_CLASS_VLAN|PTP_CLASS_L2 54 * ret a ; return PTP class 55 * 56 * ; PTP over UDP over IPv4 over 802.1Q over Ethernet 57 * test_8021q_ipv4: 58 * jneq #0x800, test_8021q_ipv6 ; ETH_P_IP ? 59 * ldb [27] ; load proto 60 * jneq #17, drop_8021q_ipv4 ; IPPROTO_UDP ? 61 * ldh [24] ; load frag offset field 62 * jset #0x1fff, drop_8021q_ipv4; don't allow fragments 63 * ldxb 4*([18]&0xf) ; load IP header len 64 * ldh [x + 20] ; load UDP dst port 65 * jneq #319, drop_8021q_ipv4 ; is port PTP_EV_PORT ? 66 * ldh [x + 26] ; load payload 67 * and #0xf ; mask PTP_CLASS_VMASK 68 * or #0x90 ; PTP_CLASS_VLAN|PTP_CLASS_IPV4 69 * ret a ; return PTP class 70 * drop_8021q_ipv4: ret #0x0 ; PTP_CLASS_NONE 71 * 72 * ; PTP over UDP over IPv6 over 802.1Q over Ethernet 73 * test_8021q_ipv6: 74 * jneq #0x86dd, drop_8021q_ipv6 ; ETH_P_IPV6 ? 75 * ldb [24] ; load proto 76 * jneq #17, drop_8021q_ipv6 ; IPPROTO_UDP ? 77 * ldh [60] ; load UDP dst port 78 * jneq #319, drop_8021q_ipv6 ; is port PTP_EV_PORT ? 79 * ldh [66] ; load payload 80 * and #0xf ; mask PTP_CLASS_VMASK 81 * or #0xa0 ; PTP_CLASS_VLAN|PTP_CLASS_IPV6 82 * ret a ; return PTP class 83 * drop_8021q_ipv6: ret #0x0 ; PTP_CLASS_NONE 84 * 85 * ; PTP over Ethernet 86 * test_ieee1588: 87 * jneq #0x88f7, drop_ieee1588 ; ETH_P_1588 ? 88 * ldb [14] ; load payload 89 * and #0x8 ; as we don't have ports here, test 90 * jneq #0x0, drop_ieee1588 ; for PTP_GEN_BIT and drop these 91 * ldh [14] ; reload payload 92 * and #0xf ; mask PTP_CLASS_VMASK 93 * or #0x40 ; PTP_CLASS_L2 94 * ret a ; return PTP class 95 * drop_ieee1588: ret #0x0 ; PTP_CLASS_NONE 96 */ 97 98#include <linux/skbuff.h> 99#include <linux/filter.h> 100#include <linux/ptp_classify.h> 101 102static struct bpf_prog *ptp_insns __read_mostly; 103 104unsigned int ptp_classify_raw(const struct sk_buff *skb) 105{ 106 return bpf_prog_run(ptp_insns, skb); 107} 108EXPORT_SYMBOL_GPL(ptp_classify_raw); 109 110struct ptp_header *ptp_parse_header(struct sk_buff *skb, unsigned int type) 111{ 112 u8 *ptr = skb_mac_header(skb); 113 114 if (type & PTP_CLASS_VLAN) 115 ptr += VLAN_HLEN; 116 117 switch (type & PTP_CLASS_PMASK) { 118 case PTP_CLASS_IPV4: 119 ptr += IPV4_HLEN(ptr) + UDP_HLEN; 120 break; 121 case PTP_CLASS_IPV6: 122 ptr += IP6_HLEN + UDP_HLEN; 123 break; 124 case PTP_CLASS_L2: 125 break; 126 default: 127 return NULL; 128 } 129 130 ptr += ETH_HLEN; 131 132 /* Ensure that the entire header is present in this packet. */ 133 if (ptr + sizeof(struct ptp_header) > skb->data + skb->len) 134 return NULL; 135 136 return (struct ptp_header *)ptr; 137} 138EXPORT_SYMBOL_GPL(ptp_parse_header); 139 140bool ptp_msg_is_sync(struct sk_buff *skb, unsigned int type) 141{ 142 struct ptp_header *hdr; 143 144 hdr = ptp_parse_header(skb, type); 145 if (!hdr) 146 return false; 147 148 return ptp_get_msgtype(hdr, type) == PTP_MSGTYPE_SYNC; 149} 150EXPORT_SYMBOL_GPL(ptp_msg_is_sync); 151 152void __init ptp_classifier_init(void) 153{ 154 static struct sock_filter ptp_filter[] __initdata = { 155 { 0x28, 0, 0, 0x0000000c }, 156 { 0x15, 0, 12, 0x00000800 }, 157 { 0x30, 0, 0, 0x00000017 }, 158 { 0x15, 0, 9, 0x00000011 }, 159 { 0x28, 0, 0, 0x00000014 }, 160 { 0x45, 7, 0, 0x00001fff }, 161 { 0xb1, 0, 0, 0x0000000e }, 162 { 0x48, 0, 0, 0x00000010 }, 163 { 0x15, 0, 4, 0x0000013f }, 164 { 0x48, 0, 0, 0x00000016 }, 165 { 0x54, 0, 0, 0x0000000f }, 166 { 0x44, 0, 0, 0x00000010 }, 167 { 0x16, 0, 0, 0x00000000 }, 168 { 0x06, 0, 0, 0x00000000 }, 169 { 0x15, 0, 9, 0x000086dd }, 170 { 0x30, 0, 0, 0x00000014 }, 171 { 0x15, 0, 6, 0x00000011 }, 172 { 0x28, 0, 0, 0x00000038 }, 173 { 0x15, 0, 4, 0x0000013f }, 174 { 0x28, 0, 0, 0x0000003e }, 175 { 0x54, 0, 0, 0x0000000f }, 176 { 0x44, 0, 0, 0x00000020 }, 177 { 0x16, 0, 0, 0x00000000 }, 178 { 0x06, 0, 0, 0x00000000 }, 179 { 0x15, 0, 32, 0x00008100 }, 180 { 0x28, 0, 0, 0x00000010 }, 181 { 0x15, 0, 7, 0x000088f7 }, 182 { 0x30, 0, 0, 0x00000012 }, 183 { 0x54, 0, 0, 0x00000008 }, 184 { 0x15, 0, 35, 0x00000000 }, 185 { 0x28, 0, 0, 0x00000012 }, 186 { 0x54, 0, 0, 0x0000000f }, 187 { 0x44, 0, 0, 0x000000c0 }, 188 { 0x16, 0, 0, 0x00000000 }, 189 { 0x15, 0, 12, 0x00000800 }, 190 { 0x30, 0, 0, 0x0000001b }, 191 { 0x15, 0, 9, 0x00000011 }, 192 { 0x28, 0, 0, 0x00000018 }, 193 { 0x45, 7, 0, 0x00001fff }, 194 { 0xb1, 0, 0, 0x00000012 }, 195 { 0x48, 0, 0, 0x00000014 }, 196 { 0x15, 0, 4, 0x0000013f }, 197 { 0x48, 0, 0, 0x0000001a }, 198 { 0x54, 0, 0, 0x0000000f }, 199 { 0x44, 0, 0, 0x00000090 }, 200 { 0x16, 0, 0, 0x00000000 }, 201 { 0x06, 0, 0, 0x00000000 }, 202 { 0x15, 0, 8, 0x000086dd }, 203 { 0x30, 0, 0, 0x00000018 }, 204 { 0x15, 0, 6, 0x00000011 }, 205 { 0x28, 0, 0, 0x0000003c }, 206 { 0x15, 0, 4, 0x0000013f }, 207 { 0x28, 0, 0, 0x00000042 }, 208 { 0x54, 0, 0, 0x0000000f }, 209 { 0x44, 0, 0, 0x000000a0 }, 210 { 0x16, 0, 0, 0x00000000 }, 211 { 0x06, 0, 0, 0x00000000 }, 212 { 0x15, 0, 7, 0x000088f7 }, 213 { 0x30, 0, 0, 0x0000000e }, 214 { 0x54, 0, 0, 0x00000008 }, 215 { 0x15, 0, 4, 0x00000000 }, 216 { 0x28, 0, 0, 0x0000000e }, 217 { 0x54, 0, 0, 0x0000000f }, 218 { 0x44, 0, 0, 0x00000040 }, 219 { 0x16, 0, 0, 0x00000000 }, 220 { 0x06, 0, 0, 0x00000000 }, 221 }; 222 struct sock_fprog_kern ptp_prog; 223 224 ptp_prog.len = ARRAY_SIZE(ptp_filter); 225 ptp_prog.filter = ptp_filter; 226 227 BUG_ON(bpf_prog_create(&ptp_insns, &ptp_prog)); 228} 229