diff options
author | Vladimir Oltean <vladimir.oltean@nxp.com> | 2022-09-08 19:48:10 +0300 |
---|---|---|
committer | David S. Miller <davem@davemloft.net> | 2022-09-09 10:59:12 +0100 |
commit | d50e41bf023454cd3441f969f18b259a90b6aa7c (patch) | |
tree | bc0ec676a44dd6a89fe2bc1dce4be66ea0aa75db /drivers/net/ethernet/mscc/ocelot.c | |
parent | 97076c3cc9fed862acb7d4ac0bb846b1bdeede85 (diff) |
net: mscc: ocelot: move more PTP code from the lib to ocelot_ptp.c
Decongest ocelot.c a bit more by moving all PTP related logic (including
timestamp processing and PTP packet traps) to ocelot_ptp.c.
Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Signed-off-by: David S. Miller <davem@davemloft.net>
Diffstat (limited to 'drivers/net/ethernet/mscc/ocelot.c')
-rw-r--r-- | drivers/net/ethernet/mscc/ocelot.c | 478 |
1 files changed, 0 insertions, 478 deletions
diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c index d1bbc48cc246..874fb2a5874e 100644 --- a/drivers/net/ethernet/mscc/ocelot.c +++ b/drivers/net/ethernet/mscc/ocelot.c @@ -6,7 +6,6 @@ */ #include <linux/dsa/ocelot.h> #include <linux/if_bridge.h> -#include <linux/ptp_classify.h> #include <soc/mscc/ocelot_vcap.h> #include "ocelot.h" #include "ocelot_vcap.h" @@ -910,211 +909,6 @@ void ocelot_phylink_mac_link_up(struct ocelot *ocelot, int port, } EXPORT_SYMBOL_GPL(ocelot_phylink_mac_link_up); -static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port, - struct sk_buff *clone) -{ - struct ocelot_port *ocelot_port = ocelot->ports[port]; - unsigned long flags; - - spin_lock_irqsave(&ocelot->ts_id_lock, flags); - - if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID || - ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) { - spin_unlock_irqrestore(&ocelot->ts_id_lock, flags); - return -EBUSY; - } - - skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS; - /* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */ - OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id; - - ocelot_port->ts_id++; - if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID) - ocelot_port->ts_id = 0; - - ocelot_port->ptp_skbs_in_flight++; - ocelot->ptp_skbs_in_flight++; - - skb_queue_tail(&ocelot_port->tx_skbs, clone); - - spin_unlock_irqrestore(&ocelot->ts_id_lock, flags); - - return 0; -} - -static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb, - unsigned int ptp_class) -{ - struct ptp_header *hdr; - u8 msgtype, twostep; - - hdr = ptp_parse_header(skb, ptp_class); - if (!hdr) - return false; - - msgtype = ptp_get_msgtype(hdr, ptp_class); - twostep = hdr->flag_field[0] & 0x2; - - if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0) - return true; - - return false; -} - -int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port, - struct sk_buff *skb, - struct sk_buff **clone) -{ - struct ocelot_port *ocelot_port = ocelot->ports[port]; - u8 ptp_cmd = ocelot_port->ptp_cmd; - unsigned int ptp_class; - int err; - - /* Don't do anything if PTP timestamping not enabled */ - if (!ptp_cmd) - return 0; - - ptp_class = ptp_classify_raw(skb); - if (ptp_class == PTP_CLASS_NONE) - return -EINVAL; - - /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */ - if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) { - if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) { - OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; - return 0; - } - - /* Fall back to two-step timestamping */ - ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; - } - - if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) { - *clone = skb_clone_sk(skb); - if (!(*clone)) - return -ENOMEM; - - err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone); - if (err) - return err; - - OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; - OCELOT_SKB_CB(*clone)->ptp_class = ptp_class; - } - - return 0; -} -EXPORT_SYMBOL(ocelot_port_txtstamp_request); - -static void ocelot_get_hwtimestamp(struct ocelot *ocelot, - struct timespec64 *ts) -{ - unsigned long flags; - u32 val; - - spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); - - /* Read current PTP time to get seconds */ - val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); - - val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM); - val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE); - ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); - ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN); - - /* Read packet HW timestamp from FIFO */ - val = ocelot_read(ocelot, SYS_PTP_TXSTAMP); - ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val); - - /* Sec has incremented since the ts was registered */ - if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC)) - ts->tv_sec--; - - spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); -} - -static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid) -{ - struct ptp_header *hdr; - - hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class); - if (WARN_ON(!hdr)) - return false; - - return seqid == ntohs(hdr->sequence_id); -} - -void ocelot_get_txtstamp(struct ocelot *ocelot) -{ - int budget = OCELOT_PTP_QUEUE_SZ; - - while (budget--) { - struct sk_buff *skb, *skb_tmp, *skb_match = NULL; - struct skb_shared_hwtstamps shhwtstamps; - u32 val, id, seqid, txport; - struct ocelot_port *port; - struct timespec64 ts; - unsigned long flags; - - val = ocelot_read(ocelot, SYS_PTP_STATUS); - - /* Check if a timestamp can be retrieved */ - if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD)) - break; - - WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL); - - /* Retrieve the ts ID and Tx port */ - id = SYS_PTP_STATUS_PTP_MESS_ID_X(val); - txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val); - seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val); - - port = ocelot->ports[txport]; - - spin_lock(&ocelot->ts_id_lock); - port->ptp_skbs_in_flight--; - ocelot->ptp_skbs_in_flight--; - spin_unlock(&ocelot->ts_id_lock); - - /* Retrieve its associated skb */ -try_again: - spin_lock_irqsave(&port->tx_skbs.lock, flags); - - skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { - if (OCELOT_SKB_CB(skb)->ts_id != id) - continue; - __skb_unlink(skb, &port->tx_skbs); - skb_match = skb; - break; - } - - spin_unlock_irqrestore(&port->tx_skbs.lock, flags); - - if (WARN_ON(!skb_match)) - continue; - - if (!ocelot_validate_ptp_skb(skb_match, seqid)) { - dev_err_ratelimited(ocelot->dev, - "port %d received stale TX timestamp for seqid %d, discarding\n", - txport, seqid); - dev_kfree_skb_any(skb); - goto try_again; - } - - /* Get the h/w timestamp */ - ocelot_get_hwtimestamp(ocelot, &ts); - - /* Set the timestamp into the skb */ - memset(&shhwtstamps, 0, sizeof(shhwtstamps)); - shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec); - skb_complete_tx_timestamp(skb_match, &shhwtstamps); - - /* Next ts */ - ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT); - } -} -EXPORT_SYMBOL(ocelot_get_txtstamp); - static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh, u32 *rval) { @@ -1497,53 +1291,6 @@ int ocelot_fdb_dump(struct ocelot *ocelot, int port, } EXPORT_SYMBOL(ocelot_fdb_dump); -static void ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter *trap) -{ - trap->key_type = OCELOT_VCAP_KEY_ETYPE; - *(__be16 *)trap->key.etype.etype.value = htons(ETH_P_1588); - *(__be16 *)trap->key.etype.etype.mask = htons(0xffff); -} - -static void -ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter *trap) -{ - trap->key_type = OCELOT_VCAP_KEY_IPV4; - trap->key.ipv4.proto.value[0] = IPPROTO_UDP; - trap->key.ipv4.proto.mask[0] = 0xff; - trap->key.ipv4.dport.value = PTP_EV_PORT; - trap->key.ipv4.dport.mask = 0xffff; -} - -static void -ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter *trap) -{ - trap->key_type = OCELOT_VCAP_KEY_IPV6; - trap->key.ipv4.proto.value[0] = IPPROTO_UDP; - trap->key.ipv4.proto.mask[0] = 0xff; - trap->key.ipv6.dport.value = PTP_EV_PORT; - trap->key.ipv6.dport.mask = 0xffff; -} - -static void -ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter *trap) -{ - trap->key_type = OCELOT_VCAP_KEY_IPV4; - trap->key.ipv4.proto.value[0] = IPPROTO_UDP; - trap->key.ipv4.proto.mask[0] = 0xff; - trap->key.ipv4.dport.value = PTP_GEN_PORT; - trap->key.ipv4.dport.mask = 0xffff; -} - -static void -ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter *trap) -{ - trap->key_type = OCELOT_VCAP_KEY_IPV6; - trap->key.ipv4.proto.value[0] = IPPROTO_UDP; - trap->key.ipv4.proto.mask[0] = 0xff; - trap->key.ipv6.dport.value = PTP_GEN_PORT; - trap->key.ipv6.dport.mask = 0xffff; -} - int ocelot_trap_add(struct ocelot *ocelot, int port, unsigned long cookie, bool take_ts, void (*populate)(struct ocelot_vcap_filter *f)) @@ -1612,231 +1359,6 @@ int ocelot_trap_del(struct ocelot *ocelot, int port, unsigned long cookie) return ocelot_vcap_filter_replace(ocelot, trap); } -static int ocelot_l2_ptp_trap_add(struct ocelot *ocelot, int port) -{ - unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot); - - return ocelot_trap_add(ocelot, port, l2_cookie, true, - ocelot_populate_l2_ptp_trap_key); -} - -static int ocelot_l2_ptp_trap_del(struct ocelot *ocelot, int port) -{ - unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot); - - return ocelot_trap_del(ocelot, port, l2_cookie); -} - -static int ocelot_ipv4_ptp_trap_add(struct ocelot *ocelot, int port) -{ - unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot); - unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot); - int err; - - err = ocelot_trap_add(ocelot, port, ipv4_ev_cookie, true, - ocelot_populate_ipv4_ptp_event_trap_key); - if (err) - return err; - - err = ocelot_trap_add(ocelot, port, ipv4_gen_cookie, false, - ocelot_populate_ipv4_ptp_general_trap_key); - if (err) - ocelot_trap_del(ocelot, port, ipv4_ev_cookie); - - return err; -} - -static int ocelot_ipv4_ptp_trap_del(struct ocelot *ocelot, int port) -{ - unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot); - unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot); - int err; - - err = ocelot_trap_del(ocelot, port, ipv4_ev_cookie); - err |= ocelot_trap_del(ocelot, port, ipv4_gen_cookie); - return err; -} - -static int ocelot_ipv6_ptp_trap_add(struct ocelot *ocelot, int port) -{ - unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot); - unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot); - int err; - - err = ocelot_trap_add(ocelot, port, ipv6_ev_cookie, true, - ocelot_populate_ipv6_ptp_event_trap_key); - if (err) - return err; - - err = ocelot_trap_add(ocelot, port, ipv6_gen_cookie, false, - ocelot_populate_ipv6_ptp_general_trap_key); - if (err) - ocelot_trap_del(ocelot, port, ipv6_ev_cookie); - - return err; -} - -static int ocelot_ipv6_ptp_trap_del(struct ocelot *ocelot, int port) -{ - unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot); - unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot); - int err; - - err = ocelot_trap_del(ocelot, port, ipv6_ev_cookie); - err |= ocelot_trap_del(ocelot, port, ipv6_gen_cookie); - return err; -} - -static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port, - bool l2, bool l4) -{ - int err; - - if (l2) - err = ocelot_l2_ptp_trap_add(ocelot, port); - else - err = ocelot_l2_ptp_trap_del(ocelot, port); - if (err) - return err; - - if (l4) { - err = ocelot_ipv4_ptp_trap_add(ocelot, port); - if (err) - goto err_ipv4; - - err = ocelot_ipv6_ptp_trap_add(ocelot, port); - if (err) - goto err_ipv6; - } else { - err = ocelot_ipv4_ptp_trap_del(ocelot, port); - - err |= ocelot_ipv6_ptp_trap_del(ocelot, port); - } - if (err) - return err; - - return 0; - -err_ipv6: - ocelot_ipv4_ptp_trap_del(ocelot, port); -err_ipv4: - if (l2) - ocelot_l2_ptp_trap_del(ocelot, port); - return err; -} - -int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr) -{ - return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config, - sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0; -} -EXPORT_SYMBOL(ocelot_hwstamp_get); - -int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr) -{ - struct ocelot_port *ocelot_port = ocelot->ports[port]; - bool l2 = false, l4 = false; - struct hwtstamp_config cfg; - int err; - - if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg))) - return -EFAULT; - - /* Tx type sanity check */ - switch (cfg.tx_type) { - case HWTSTAMP_TX_ON: - ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; - break; - case HWTSTAMP_TX_ONESTEP_SYNC: - /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we - * need to update the origin time. - */ - ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP; - break; - case HWTSTAMP_TX_OFF: - ocelot_port->ptp_cmd = 0; - break; - default: - return -ERANGE; - } - - mutex_lock(&ocelot->ptp_lock); - - switch (cfg.rx_filter) { - case HWTSTAMP_FILTER_NONE: - break; - case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: - case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: - case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: - l4 = true; - break; - case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: - case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: - case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: - l2 = true; - break; - case HWTSTAMP_FILTER_PTP_V2_EVENT: - case HWTSTAMP_FILTER_PTP_V2_SYNC: - case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: - l2 = true; - l4 = true; - break; - default: - mutex_unlock(&ocelot->ptp_lock); - return -ERANGE; - } - - err = ocelot_setup_ptp_traps(ocelot, port, l2, l4); - if (err) { - mutex_unlock(&ocelot->ptp_lock); - return err; - } - - if (l2 && l4) - cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; - else if (l2) - cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; - else if (l4) - cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT; - else - cfg.rx_filter = HWTSTAMP_FILTER_NONE; - - /* Commit back the result & save it */ - memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg)); - mutex_unlock(&ocelot->ptp_lock); - - return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; -} -EXPORT_SYMBOL(ocelot_hwstamp_set); - -int ocelot_get_ts_info(struct ocelot *ocelot, int port, - struct ethtool_ts_info *info) -{ - info->phc_index = ocelot->ptp_clock ? - ptp_clock_index(ocelot->ptp_clock) : -1; - if (info->phc_index == -1) { - info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE | - SOF_TIMESTAMPING_RX_SOFTWARE | - SOF_TIMESTAMPING_SOFTWARE; - return 0; - } - info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE | - SOF_TIMESTAMPING_RX_SOFTWARE | - SOF_TIMESTAMPING_SOFTWARE | - SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_RAW_HARDWARE; - info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) | - BIT(HWTSTAMP_TX_ONESTEP_SYNC); - info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | - BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) | - BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | - BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT); - - return 0; -} -EXPORT_SYMBOL(ocelot_get_ts_info); - static u32 ocelot_get_bond_mask(struct ocelot *ocelot, struct net_device *bond) { u32 mask = 0; |