diff --git a/drivers/net/dsa/mv88e6xxx/hwtstamp.c b/drivers/net/dsa/mv88e6xxx/hwtstamp.c index a4c488b12e8f..094d17a1d037 100644 --- a/drivers/net/dsa/mv88e6xxx/hwtstamp.c +++ b/drivers/net/dsa/mv88e6xxx/hwtstamp.c @@ -211,49 +211,20 @@ int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port, -EFAULT : 0; } -/* Get the start of the PTP header in this skb */ -static u8 *parse_ptp_header(struct sk_buff *skb, unsigned int type) -{ - u8 *data = skb_mac_header(skb); - unsigned int offset = 0; - - if (type & PTP_CLASS_VLAN) - offset += VLAN_HLEN; - - switch (type & PTP_CLASS_PMASK) { - case PTP_CLASS_IPV4: - offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; - break; - case PTP_CLASS_IPV6: - offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; - break; - case PTP_CLASS_L2: - offset += ETH_HLEN; - break; - default: - return NULL; - } - - /* Ensure that the entire header is present in this packet. */ - if (skb->len + ETH_HLEN < offset + 34) - return NULL; - - return data + offset; -} - /* Returns a pointer to the PTP header if the caller should time stamp, * or NULL if the caller should not. */ -static u8 *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, int port, - struct sk_buff *skb, unsigned int type) +static struct ptp_header *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, + int port, struct sk_buff *skb, + unsigned int type) { struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; - u8 *hdr; + struct ptp_header *hdr; if (!chip->info->ptp_support) return NULL; - hdr = parse_ptp_header(skb, type); + hdr = ptp_parse_header(skb, type); if (!hdr) return NULL; @@ -275,12 +246,11 @@ static int mv88e6xxx_ts_valid(u16 status) static int seq_match(struct sk_buff *skb, u16 ts_seqid) { unsigned int type = SKB_PTP_TYPE(skb); - u8 *hdr = parse_ptp_header(skb, type); - __be16 *seqid; + struct ptp_header *hdr; - seqid = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID); + hdr = ptp_parse_header(skb, type); - return ts_seqid == ntohs(*seqid); + return ts_seqid == ntohs(hdr->sequence_id); } static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip, @@ -357,9 +327,9 @@ static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip, &ps->rx_queue2); } -static int is_pdelay_resp(u8 *msgtype) +static int is_pdelay_resp(const struct ptp_header *hdr) { - return (*msgtype & 0xf) == 3; + return (hdr->tsmt & 0xf) == 3; } bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port, @@ -367,7 +337,7 @@ bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port, { struct mv88e6xxx_port_hwtstamp *ps; struct mv88e6xxx_chip *chip; - u8 *hdr; + struct ptp_header *hdr; chip = ds->priv; ps = &chip->port_hwtstamp[port]; @@ -503,8 +473,7 @@ bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port, { struct mv88e6xxx_chip *chip = ds->priv; struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; - __be16 *seq_ptr; - u8 *hdr; + struct ptp_header *hdr; if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP)) return false; @@ -513,15 +482,13 @@ bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port, if (!hdr) return false; - seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID); - if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) return false; ps->tx_skb = clone; ps->tx_tstamp_start = jiffies; - ps->tx_seq_id = be16_to_cpup(seq_ptr); + ps->tx_seq_id = be16_to_cpu(hdr->sequence_id); ptp_schedule_worker(chip->ptp_clock, 0); return true; diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c index 9650562fc0ef..ca8090a28dec 100644 --- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c +++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c @@ -314,11 +314,9 @@ static int mlxsw_sp_ptp_parse(struct sk_buff *skb, u8 *p_message_type, u16 *p_sequence_id) { - unsigned int offset = 0; unsigned int ptp_class; - u8 *data; + struct ptp_header *hdr; - data = skb_mac_header(skb); ptp_class = ptp_classify_raw(skb); switch (ptp_class & PTP_CLASS_VMASK) { @@ -329,30 +327,14 @@ static int mlxsw_sp_ptp_parse(struct sk_buff *skb, return -ERANGE; } - 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 + offset) + UDP_HLEN; - break; - case PTP_CLASS_IPV6: - offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; - break; - case PTP_CLASS_L2: - offset += ETH_HLEN; - break; - default: - return -ERANGE; - } - - /* PTP header is 34 bytes. */ - if (skb->len < offset + 34) + hdr = ptp_parse_header(skb, ptp_class); + if (!hdr) return -EINVAL; - *p_message_type = data[offset] & 0x0f; - *p_domain_number = data[offset + 4]; - *p_sequence_id = (u16)(data[offset + 30]) << 8 | data[offset + 31]; + *p_message_type = ptp_get_msgtype(hdr, ptp_class); + *p_domain_number = hdr->domain_number; + *p_sequence_id = be16_to_cpu(hdr->sequence_id); + return 0; } diff --git a/drivers/net/ethernet/ti/am65-cpts.c b/drivers/net/ethernet/ti/am65-cpts.c index c59a289e428c..365b5b9c6897 100644 --- a/drivers/net/ethernet/ti/am65-cpts.c +++ b/drivers/net/ethernet/ti/am65-cpts.c @@ -748,42 +748,23 @@ EXPORT_SYMBOL_GPL(am65_cpts_rx_enable); static int am65_skb_get_mtype_seqid(struct sk_buff *skb, u32 *mtype_seqid) { unsigned int ptp_class = ptp_classify_raw(skb); - u8 *msgtype, *data = skb->data; - unsigned int offset = 0; - __be16 *seqid; + struct ptp_header *hdr; + u8 msgtype; + u16 seqid; if (ptp_class == PTP_CLASS_NONE) return 0; - 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 + offset) + UDP_HLEN; - break; - case PTP_CLASS_IPV6: - offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; - break; - case PTP_CLASS_L2: - offset += ETH_HLEN; - break; - default: - return 0; - } - - if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) + hdr = ptp_parse_header(skb, ptp_class); + if (!hdr) return 0; - if (unlikely(ptp_class & PTP_CLASS_V1)) - msgtype = data + offset + OFF_PTP_CONTROL; - else - msgtype = data + offset; + msgtype = ptp_get_msgtype(hdr, ptp_class); + seqid = ntohs(hdr->sequence_id); - seqid = (__be16 *)(data + offset + OFF_PTP_SEQUENCE_ID); - *mtype_seqid = (*msgtype << AM65_CPTS_EVENT_1_MESSAGE_TYPE_SHIFT) & + *mtype_seqid = (msgtype << AM65_CPTS_EVENT_1_MESSAGE_TYPE_SHIFT) & AM65_CPTS_EVENT_1_MESSAGE_TYPE_MASK; - *mtype_seqid |= (ntohs(*seqid) & AM65_CPTS_EVENT_1_SEQUENCE_ID_MASK); + *mtype_seqid |= (seqid & AM65_CPTS_EVENT_1_SEQUENCE_ID_MASK); return 1; } diff --git a/drivers/net/ethernet/ti/cpts.c b/drivers/net/ethernet/ti/cpts.c index 7c55d395de2c..d1fc7955d422 100644 --- a/drivers/net/ethernet/ti/cpts.c +++ b/drivers/net/ethernet/ti/cpts.c @@ -446,41 +446,22 @@ static const struct ptp_clock_info cpts_info = { static int cpts_skb_get_mtype_seqid(struct sk_buff *skb, u32 *mtype_seqid) { unsigned int ptp_class = ptp_classify_raw(skb); - u8 *msgtype, *data = skb->data; - unsigned int offset = 0; - u16 *seqid; + struct ptp_header *hdr; + u8 msgtype; + u16 seqid; if (ptp_class == PTP_CLASS_NONE) return 0; - 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 + offset) + UDP_HLEN; - break; - case PTP_CLASS_IPV6: - offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; - break; - case PTP_CLASS_L2: - offset += ETH_HLEN; - break; - default: - return 0; - } - - if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) + hdr = ptp_parse_header(skb, ptp_class); + if (!hdr) return 0; - if (unlikely(ptp_class & PTP_CLASS_V1)) - msgtype = data + offset + OFF_PTP_CONTROL; - else - msgtype = data + offset; + msgtype = ptp_get_msgtype(hdr, ptp_class); + seqid = ntohs(hdr->sequence_id); - seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID); - *mtype_seqid = (*msgtype & MESSAGE_TYPE_MASK) << MESSAGE_TYPE_SHIFT; - *mtype_seqid |= (ntohs(*seqid) & SEQUENCE_ID_MASK) << SEQUENCE_ID_SHIFT; + *mtype_seqid = (msgtype & MESSAGE_TYPE_MASK) << MESSAGE_TYPE_SHIFT; + *mtype_seqid |= (seqid & SEQUENCE_ID_MASK) << SEQUENCE_ID_SHIFT; return 1; } @@ -528,6 +509,11 @@ void cpts_rx_timestamp(struct cpts *cpts, struct sk_buff *skb) int ret; u64 ns; + /* cpts_rx_timestamp() is called before eth_type_trans(), so + * skb MAC Hdr properties are not configured yet. Hence need to + * reset skb MAC header here + */ + skb_reset_mac_header(skb); ret = cpts_skb_get_mtype_seqid(skb, &skb_cb->skb_mtype_seqid); if (!ret) return; diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c index 50fb7d16b75a..fc3d747eba55 100644 --- a/drivers/net/phy/dp83640.c +++ b/drivers/net/phy/dp83640.c @@ -798,51 +798,32 @@ static int decode_evnt(struct dp83640_private *dp83640, return parsed; } -#define DP83640_PACKET_HASH_OFFSET 20 #define DP83640_PACKET_HASH_LEN 10 static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts) { - unsigned int offset = 0; - u8 *msgtype, *data = skb_mac_header(skb); - __be16 *seqid; + struct ptp_header *hdr; + u8 msgtype; + u16 seqid; u16 hash; /* check sequenceID, messageType, 12 bit hash of offset 20-29 */ - if (type & PTP_CLASS_VLAN) - offset += VLAN_HLEN; - - switch (type & PTP_CLASS_PMASK) { - case PTP_CLASS_IPV4: - offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; - break; - case PTP_CLASS_IPV6: - offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; - break; - case PTP_CLASS_L2: - offset += ETH_HLEN; - break; - default: - return 0; - } - - if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) + hdr = ptp_parse_header(skb, type); + if (!hdr) return 0; - if (unlikely(type & PTP_CLASS_V1)) - msgtype = data + offset + OFF_PTP_CONTROL; - else - msgtype = data + offset; - if (rxts->msgtype != (*msgtype & 0xf)) + msgtype = ptp_get_msgtype(hdr, type); + + if (rxts->msgtype != (msgtype & 0xf)) return 0; - seqid = (__be16 *)(data + offset + OFF_PTP_SEQUENCE_ID); - if (rxts->seqid != ntohs(*seqid)) + seqid = be16_to_cpu(hdr->sequence_id); + if (rxts->seqid != seqid) return 0; hash = ether_crc(DP83640_PACKET_HASH_LEN, - data + offset + DP83640_PACKET_HASH_OFFSET) >> 20; + (unsigned char *)&hdr->source_port_identity) >> 20; if (rxts->hash != hash) return 0; @@ -982,35 +963,16 @@ static void decode_status_frame(struct dp83640_private *dp83640, static int is_sync(struct sk_buff *skb, int type) { - u8 *data = skb->data, *msgtype; - unsigned int offset = 0; + struct ptp_header *hdr; + u8 msgtype; - if (type & PTP_CLASS_VLAN) - offset += VLAN_HLEN; - - switch (type & PTP_CLASS_PMASK) { - case PTP_CLASS_IPV4: - offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; - break; - case PTP_CLASS_IPV6: - offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; - break; - case PTP_CLASS_L2: - offset += ETH_HLEN; - break; - default: - return 0; - } - - if (type & PTP_CLASS_V1) - offset += OFF_PTP_CONTROL; - - if (skb->len < offset + 1) + hdr = ptp_parse_header(skb, type); + if (!hdr) return 0; - msgtype = data + offset; + msgtype = ptp_get_msgtype(hdr, type); - return (*msgtype & 0xf) == 0; + return (msgtype & 0xf) == 0; } static void dp83640_free_clocks(void) diff --git a/drivers/ptp/ptp_ines.c b/drivers/ptp/ptp_ines.c index 7711651ff19e..d726c589edda 100644 --- a/drivers/ptp/ptp_ines.c +++ b/drivers/ptp/ptp_ines.c @@ -93,9 +93,6 @@ MODULE_LICENSE("GPL"); #define TC_E2E_PTP_V2 2 #define TC_P2P_PTP_V2 3 -#define OFF_PTP_CLOCK_ID 20 -#define OFF_PTP_PORT_NUM 28 - #define PHY_SPEED_10 0 #define PHY_SPEED_100 1 #define PHY_SPEED_1000 2 @@ -443,57 +440,41 @@ static void ines_link_state(struct mii_timestamper *mii_ts, static bool ines_match(struct sk_buff *skb, unsigned int ptp_class, struct ines_timestamp *ts, struct device *dev) { - u8 *msgtype, *data = skb_mac_header(skb); - unsigned int offset = 0; - __be16 *portn, *seqid; - __be64 *clkid; + struct ptp_header *hdr; + u16 portn, seqid; + u8 msgtype; + u64 clkid; if (unlikely(ptp_class & PTP_CLASS_V1)) return false; - 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 + offset) + UDP_HLEN; - break; - case PTP_CLASS_IPV6: - offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; - break; - case PTP_CLASS_L2: - offset += ETH_HLEN; - break; - default: - return false; - } - - if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) + hdr = ptp_parse_header(skb, ptp_class); + if (!hdr) return false; - msgtype = data + offset; - clkid = (__be64 *)(data + offset + OFF_PTP_CLOCK_ID); - portn = (__be16 *)(data + offset + OFF_PTP_PORT_NUM); - seqid = (__be16 *)(data + offset + OFF_PTP_SEQUENCE_ID); + msgtype = ptp_get_msgtype(hdr, ptp_class); + clkid = be64_to_cpup((__be64 *)&hdr->source_port_identity.clock_identity.id[0]); + portn = be16_to_cpu(hdr->source_port_identity.port_number); + seqid = be16_to_cpu(hdr->sequence_id); - if (tag_to_msgtype(ts->tag & 0x7) != (*msgtype & 0xf)) { + if (tag_to_msgtype(ts->tag & 0x7) != msgtype) { dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n", - tag_to_msgtype(ts->tag & 0x7), *msgtype & 0xf); + tag_to_msgtype(ts->tag & 0x7), msgtype); return false; } - if (cpu_to_be64(ts->clkid) != *clkid) { + if (ts->clkid != clkid) { dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n", - cpu_to_be64(ts->clkid), *clkid); + ts->clkid, clkid); return false; } - if (ts->portnum != ntohs(*portn)) { + if (ts->portnum != portn) { dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n", - ts->portnum, ntohs(*portn)); + ts->portnum, portn); return false; } - if (ts->seqid != ntohs(*seqid)) { + if (ts->seqid != seqid) { dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n", - ts->seqid, ntohs(*seqid)); + ts->seqid, seqid); return false; } @@ -694,35 +675,16 @@ static void ines_txtstamp_work(struct work_struct *work) static bool is_sync_pdelay_resp(struct sk_buff *skb, int type) { - u8 *data = skb->data, *msgtype; - unsigned int offset = 0; + struct ptp_header *hdr; + u8 msgtype; - if (type & PTP_CLASS_VLAN) - offset += VLAN_HLEN; + hdr = ptp_parse_header(skb, type); + if (!hdr) + return false; - switch (type & PTP_CLASS_PMASK) { - case PTP_CLASS_IPV4: - offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; - break; - case PTP_CLASS_IPV6: - offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; - break; - case PTP_CLASS_L2: - offset += ETH_HLEN; - break; - default: - return 0; - } + msgtype = ptp_get_msgtype(hdr, type); - if (type & PTP_CLASS_V1) - offset += OFF_PTP_CONTROL; - - if (skb->len < offset + 1) - return 0; - - msgtype = data + offset; - - switch ((*msgtype & 0xf)) { + switch ((msgtype & 0xf)) { case SYNC: case PDELAY_RESP: return true; diff --git a/include/linux/ptp_classify.h b/include/linux/ptp_classify.h index dd00fa41f7e7..8437307cca8c 100644 --- a/include/linux/ptp_classify.h +++ b/include/linux/ptp_classify.h @@ -36,7 +36,6 @@ #define OFF_PTP_SOURCE_UUID 22 /* PTPv1 only */ #define OFF_PTP_SEQUENCE_ID 30 -#define OFF_PTP_CONTROL 32 /* PTPv1 only */ /* Below defines should actually be removed at some point in time. */ #define IP6_HLEN 40 @@ -44,6 +43,30 @@ #define OFF_IHL 14 #define IPV4_HLEN(data) (((struct iphdr *)(data + OFF_IHL))->ihl << 2) +struct clock_identity { + u8 id[8]; +} __packed; + +struct port_identity { + struct clock_identity clock_identity; + __be16 port_number; +} __packed; + +struct ptp_header { + u8 tsmt; /* transportSpecific | messageType */ + u8 ver; /* reserved | versionPTP */ + __be16 message_length; + u8 domain_number; + u8 reserved1; + u8 flag_field[2]; + __be64 correction; + __be32 reserved2; + struct port_identity source_port_identity; + __be16 sequence_id; + u8 control; + u8 log_message_interval; +} __packed; + #if defined(CONFIG_NET_PTP_CLASSIFY) /** * ptp_classify_raw - classify a PTP packet @@ -57,6 +80,46 @@ */ unsigned int ptp_classify_raw(const struct sk_buff *skb); +/** + * ptp_parse_header - Get pointer to the PTP v2 header + * @skb: packet buffer + * @type: type of the packet (see ptp_classify_raw()) + * + * This function takes care of the VLAN, UDP, IPv4 and IPv6 headers. The length + * is checked. + * + * Note, internally skb_mac_header() is used. Make sure that the @skb is + * initialized accordingly. + * + * Return: Pointer to the ptp v2 header or NULL if not found + */ +struct ptp_header *ptp_parse_header(struct sk_buff *skb, unsigned int type); + +/** + * ptp_get_msgtype - Extract ptp message type from given header + * @hdr: ptp header + * @type: type of the packet (see ptp_classify_raw()) + * + * This function returns the message type for a given ptp header. It takes care + * of the different ptp header versions (v1 or v2). + * + * Return: The message type + */ +static inline u8 ptp_get_msgtype(const struct ptp_header *hdr, + unsigned int type) +{ + u8 msgtype; + + if (unlikely(type & PTP_CLASS_V1)) { + /* msg type is located at the control field for ptp v1 */ + msgtype = hdr->control; + } else { + msgtype = hdr->tsmt & 0x0f; + } + + return msgtype; +} + void __init ptp_classifier_init(void); #else static inline void ptp_classifier_init(void) @@ -66,5 +129,10 @@ static inline unsigned int ptp_classify_raw(struct sk_buff *skb) { return PTP_CLASS_NONE; } +static inline struct ptp_header *ptp_parse_header(struct sk_buff *skb, + unsigned int type) +{ + return NULL; +} #endif #endif /* _PTP_CLASSIFY_H_ */ diff --git a/net/core/ptp_classifier.c b/net/core/ptp_classifier.c index d964a5147f22..e33fde06d528 100644 --- a/net/core/ptp_classifier.c +++ b/net/core/ptp_classifier.c @@ -107,6 +107,36 @@ unsigned int ptp_classify_raw(const struct sk_buff *skb) } EXPORT_SYMBOL_GPL(ptp_classify_raw); +struct ptp_header *ptp_parse_header(struct sk_buff *skb, unsigned int type) +{ + u8 *ptr = skb_mac_header(skb); + + if (type & PTP_CLASS_VLAN) + ptr += VLAN_HLEN; + + switch (type & PTP_CLASS_PMASK) { + case PTP_CLASS_IPV4: + ptr += IPV4_HLEN(ptr) + UDP_HLEN; + break; + case PTP_CLASS_IPV6: + ptr += IP6_HLEN + UDP_HLEN; + break; + case PTP_CLASS_L2: + break; + default: + return NULL; + } + + ptr += ETH_HLEN; + + /* Ensure that the entire header is present in this packet. */ + if (ptr + sizeof(struct ptp_header) > skb->data + skb->len) + return NULL; + + return (struct ptp_header *)ptr; +} +EXPORT_SYMBOL_GPL(ptp_parse_header); + void __init ptp_classifier_init(void) { static struct sock_filter ptp_filter[] __initdata = {