diff --git a/drivers/net/ethernet/ti/cpts.c b/drivers/net/ethernet/ti/cpts.c index 6b56f85951e5..ab92f67da035 100644 --- a/drivers/net/ethernet/ti/cpts.c +++ b/drivers/net/ethernet/ti/cpts.c @@ -256,23 +256,21 @@ static int cpts_match(struct sk_buff *skb, unsigned int ptp_class, u16 ts_seqid, u8 ts_msgtype) { u16 *seqid; - unsigned int offset; + unsigned int offset = 0; u8 *msgtype, *data = skb->data; - switch (ptp_class) { - case PTP_CLASS_V1_IPV4: - case PTP_CLASS_V2_IPV4: - offset = ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN; + if (ptp_class & PTP_CLASS_VLAN) + offset += VLAN_HLEN; + + switch (ptp_class & PTP_CLASS_PMASK) { + case PTP_CLASS_IPV4: + offset += ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN; break; - case PTP_CLASS_V1_IPV6: - case PTP_CLASS_V2_IPV6: - offset = OFF_PTP6; + case PTP_CLASS_IPV6: + offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; break; - case PTP_CLASS_V2_L2: - offset = ETH_HLEN; - break; - case PTP_CLASS_V2_VLAN: - offset = ETH_HLEN + VLAN_HLEN; + case PTP_CLASS_L2: + offset += ETH_HLEN; break; default: return 0; diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c index 293ad064905d..53bd1af68422 100644 --- a/drivers/net/phy/dp83640.c +++ b/drivers/net/phy/dp83640.c @@ -856,20 +856,18 @@ static int is_sync(struct sk_buff *skb, int type) u8 *data = skb->data, *msgtype; unsigned int offset = 0; - switch (type) { - case PTP_CLASS_V1_IPV4: - case PTP_CLASS_V2_IPV4: - offset = ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN; + if (type & PTP_CLASS_VLAN) + offset += VLAN_HLEN; + + switch (type & PTP_CLASS_PMASK) { + case PTP_CLASS_IPV4: + offset += ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN; break; - case PTP_CLASS_V1_IPV6: - case PTP_CLASS_V2_IPV6: - offset = OFF_PTP6; + case PTP_CLASS_IPV6: + offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; break; - case PTP_CLASS_V2_L2: - offset = ETH_HLEN; - break; - case PTP_CLASS_V2_VLAN: - offset = ETH_HLEN + VLAN_HLEN; + case PTP_CLASS_L2: + offset += ETH_HLEN; break; default: return 0; @@ -889,25 +887,23 @@ static int is_sync(struct sk_buff *skb, int type) static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts) { u16 *seqid; - unsigned int offset; + unsigned int offset = 0; u8 *msgtype, *data = skb_mac_header(skb); /* check sequenceID, messageType, 12 bit hash of offset 20-29 */ - switch (type) { - case PTP_CLASS_V1_IPV4: - case PTP_CLASS_V2_IPV4: - offset = ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN; + if (type & PTP_CLASS_VLAN) + offset += VLAN_HLEN; + + switch (type & PTP_CLASS_PMASK) { + case PTP_CLASS_IPV4: + offset += ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN; break; - case PTP_CLASS_V1_IPV6: - case PTP_CLASS_V2_IPV6: - offset = OFF_PTP6; + case PTP_CLASS_IPV6: + offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; break; - case PTP_CLASS_V2_L2: - offset = ETH_HLEN; - break; - case PTP_CLASS_V2_VLAN: - offset = ETH_HLEN + VLAN_HLEN; + case PTP_CLASS_L2: + offset += ETH_HLEN; break; default: return 0; diff --git a/include/linux/ptp_classify.h b/include/linux/ptp_classify.h index 7dfed71d76a6..159c987b1853 100644 --- a/include/linux/ptp_classify.h +++ b/include/linux/ptp_classify.h @@ -33,8 +33,8 @@ #define PTP_CLASS_IPV4 0x10 /* event in an IPV4 UDP packet */ #define PTP_CLASS_IPV6 0x20 /* event in an IPV6 UDP packet */ #define PTP_CLASS_L2 0x30 /* event in a L2 packet */ -#define PTP_CLASS_VLAN 0x40 /* event in a VLAN tagged L2 packet */ -#define PTP_CLASS_PMASK 0xf0 /* mask for the packet type field */ +#define PTP_CLASS_PMASK 0x30 /* mask for the packet type field */ +#define PTP_CLASS_VLAN 0x40 /* event in a VLAN tagged packet */ #define PTP_CLASS_V1_IPV4 (PTP_CLASS_V1 | PTP_CLASS_IPV4) #define PTP_CLASS_V1_IPV6 (PTP_CLASS_V1 | PTP_CLASS_IPV6) /* probably DNE */ @@ -54,7 +54,6 @@ #define IP6_HLEN 40 #define UDP_HLEN 8 #define OFF_IHL 14 -#define OFF_PTP6 (ETH_HLEN + IP6_HLEN + UDP_HLEN) #define IPV4_HLEN(data) (((struct iphdr *)(data + OFF_IHL))->ihl << 2) #if defined(CONFIG_NET_PTP_CLASSIFY) diff --git a/net/core/ptp_classifier.c b/net/core/ptp_classifier.c index d3027a73fd4b..12ab7b4be609 100644 --- a/net/core/ptp_classifier.c +++ b/net/core/ptp_classifier.c @@ -52,15 +52,44 @@ * test_8021q: * jneq #0x8100, test_ieee1588 ; ETH_P_8021Q ? * ldh [16] ; load inner type - * jneq #0x88f7, drop_ieee1588 ; ETH_P_1588 ? + * jneq #0x88f7, test_8021q_ipv4 ; ETH_P_1588 ? * ldb [18] ; load payload * and #0x8 ; as we don't have ports here, test * jneq #0x0, drop_ieee1588 ; for PTP_GEN_BIT and drop these * ldh [18] ; reload payload * and #0xf ; mask PTP_CLASS_VMASK - * or #0x40 ; PTP_CLASS_V2_VLAN + * or #0x70 ; PTP_CLASS_VLAN|PTP_CLASS_L2 * ret a ; return PTP class * + * ; PTP over UDP over IPv4 over 802.1Q over Ethernet + * test_8021q_ipv4: + * jneq #0x800, test_8021q_ipv6 ; ETH_P_IP ? + * ldb [27] ; load proto + * jneq #17, drop_8021q_ipv4 ; IPPROTO_UDP ? + * ldh [24] ; load frag offset field + * jset #0x1fff, drop_8021q_ipv4; don't allow fragments + * ldxb 4*([18]&0xf) ; load IP header len + * ldh [x + 20] ; load UDP dst port + * jneq #319, drop_8021q_ipv4 ; is port PTP_EV_PORT ? + * ldh [x + 26] ; load payload + * and #0xf ; mask PTP_CLASS_VMASK + * or #0x50 ; PTP_CLASS_VLAN|PTP_CLASS_IPV4 + * ret a ; return PTP class + * drop_8021q_ipv4: ret #0x0 ; PTP_CLASS_NONE + * + * ; PTP over UDP over IPv6 over 802.1Q over Ethernet + * test_8021q_ipv6: + * jneq #0x86dd, drop_8021q_ipv6 ; ETH_P_IPV6 ? + * ldb [24] ; load proto + * jneq #17, drop_8021q_ipv6 ; IPPROTO_UDP ? + * ldh [60] ; load UDP dst port + * jneq #319, drop_8021q_ipv6 ; is port PTP_EV_PORT ? + * ldh [66] ; load payload + * and #0xf ; mask PTP_CLASS_VMASK + * or #0x60 ; PTP_CLASS_VLAN|PTP_CLASS_IPV6 + * ret a ; return PTP class + * drop_8021q_ipv6: ret #0x0 ; PTP_CLASS_NONE + * * ; PTP over Ethernet * test_ieee1588: * jneq #0x88f7, drop_ieee1588 ; ETH_P_1588 ? @@ -113,16 +142,39 @@ void __init ptp_classifier_init(void) { 0x44, 0, 0, 0x00000020 }, { 0x16, 0, 0, 0x00000000 }, { 0x06, 0, 0, 0x00000000 }, - { 0x15, 0, 9, 0x00008100 }, + { 0x15, 0, 32, 0x00008100 }, { 0x28, 0, 0, 0x00000010 }, - { 0x15, 0, 15, 0x000088f7 }, + { 0x15, 0, 7, 0x000088f7 }, { 0x30, 0, 0, 0x00000012 }, { 0x54, 0, 0, 0x00000008 }, - { 0x15, 0, 12, 0x00000000 }, + { 0x15, 0, 35, 0x00000000 }, { 0x28, 0, 0, 0x00000012 }, { 0x54, 0, 0, 0x0000000f }, - { 0x44, 0, 0, 0x00000040 }, + { 0x44, 0, 0, 0x00000070 }, { 0x16, 0, 0, 0x00000000 }, + { 0x15, 0, 12, 0x00000800 }, + { 0x30, 0, 0, 0x0000001b }, + { 0x15, 0, 9, 0x00000011 }, + { 0x28, 0, 0, 0x00000018 }, + { 0x45, 7, 0, 0x00001fff }, + { 0xb1, 0, 0, 0x00000012 }, + { 0x48, 0, 0, 0x00000014 }, + { 0x15, 0, 4, 0x0000013f }, + { 0x48, 0, 0, 0x0000001a }, + { 0x54, 0, 0, 0x0000000f }, + { 0x44, 0, 0, 0x00000050 }, + { 0x16, 0, 0, 0x00000000 }, + { 0x06, 0, 0, 0x00000000 }, + { 0x15, 0, 8, 0x000086dd }, + { 0x30, 0, 0, 0x00000018 }, + { 0x15, 0, 6, 0x00000011 }, + { 0x28, 0, 0, 0x0000003c }, + { 0x15, 0, 4, 0x0000013f }, + { 0x28, 0, 0, 0x00000042 }, + { 0x54, 0, 0, 0x0000000f }, + { 0x44, 0, 0, 0x00000060 }, + { 0x16, 0, 0, 0x00000000 }, + { 0x06, 0, 0, 0x00000000 }, { 0x15, 0, 7, 0x000088f7 }, { 0x30, 0, 0, 0x0000000e }, { 0x54, 0, 0, 0x00000008 },