Commit d50e41bf authored by Vladimir Oltean's avatar Vladimir Oltean Committed by David S. Miller
Browse files

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: default avatarVladimir Oltean <vladimir.oltean@nxp.com>
Signed-off-by: default avatarDavid S. Miller <davem@davemloft.net>
parent 97076c3c

drivers/net/ethernet/mscc/ocelot.c

+0 −478
Original line number Diff line number Diff line
@@ -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;

drivers/net/ethernet/mscc/ocelot_ptp.c

+481 −0

File changed.

Preview size limit exceeded, changes collapsed.