WSL2-Linux-Kernel/drivers/net/dsa/hirschmann/hellcreek_hwtstamp.c

// SPDX-License-Identifier: (GPL-2.0 OR MIT)
/*
 * DSA driver for:
 * Hirschmann Hellcreek TSN switch.
 *
 * Copyright (C) 2019,2020 Hochschule Offenburg
 * Copyright (C) 2019,2020 Linutronix GmbH
 * Authors: Kamil Alkhouri <kamil.alkhouri@hs-offenburg.de>
 *	    Kurt Kanzenbach <kurt@linutronix.de>
 */

#include <linux/ptp_classify.h>

#include "hellcreek.h"
#include "hellcreek_hwtstamp.h"
#include "hellcreek_ptp.h"

int hellcreek_get_ts_info(struct dsa_switch *ds, int port,
			  struct ethtool_ts_info *info)
{
	struct hellcreek *hellcreek = ds->priv;

	info->phc_index = hellcreek->ptp_clock ?
		ptp_clock_index(hellcreek->ptp_clock) : -1;
	info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
		SOF_TIMESTAMPING_RX_HARDWARE |
		SOF_TIMESTAMPING_RAW_HARDWARE;

	/* enabled tx timestamping */
	info->tx_types = BIT(HWTSTAMP_TX_ON);

	/* L2 & L4 PTPv2 event rx messages are timestamped */
	info->rx_filters = BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);

	return 0;
}

/* Enabling/disabling TX and RX HW timestamping for different PTP messages is
 * not available in the switch. Thus, this function only serves as a check if
 * the user requested what is actually available or not
 */
static int hellcreek_set_hwtstamp_config(struct hellcreek *hellcreek, int port,
					 struct hwtstamp_config *config)
{
	struct hellcreek_port_hwtstamp *ps =
		&hellcreek->ports[port].port_hwtstamp;
	bool tx_tstamp_enable = false;
	bool rx_tstamp_enable = false;

	/* Interaction with the timestamp hardware is prevented here.  It is
	 * enabled when this config function ends successfully
	 */
	clear_bit_unlock(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);

	/* Reserved for future extensions */
	if (config->flags)
		return -EINVAL;

	switch (config->tx_type) {
	case HWTSTAMP_TX_ON:
		tx_tstamp_enable = true;
		break;

	/* TX HW timestamping can't be disabled on the switch */
	case HWTSTAMP_TX_OFF:
		config->tx_type = HWTSTAMP_TX_ON;
		break;

	default:
		return -ERANGE;
	}

	switch (config->rx_filter) {
	/* RX HW timestamping can't be disabled on the switch */
	case HWTSTAMP_FILTER_NONE:
		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
		break;

	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
	case HWTSTAMP_FILTER_PTP_V2_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_SYNC:
	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
		rx_tstamp_enable = true;
		break;

	/* RX HW timestamping can't be enabled for all messages on the switch */
	case HWTSTAMP_FILTER_ALL:
		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
		break;

	default:
		return -ERANGE;
	}

	if (!tx_tstamp_enable)
		return -ERANGE;

	if (!rx_tstamp_enable)
		return -ERANGE;

	/* If this point is reached, then the requested hwtstamp config is
	 * compatible with the hwtstamp offered by the switch.  Therefore,
	 * enable the interaction with the HW timestamping
	 */
	set_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);

	return 0;
}

int hellcreek_port_hwtstamp_set(struct dsa_switch *ds, int port,
				struct ifreq *ifr)
{
	struct hellcreek *hellcreek = ds->priv;
	struct hellcreek_port_hwtstamp *ps;
	struct hwtstamp_config config;
	int err;

	ps = &hellcreek->ports[port].port_hwtstamp;

	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
		return -EFAULT;

	err = hellcreek_set_hwtstamp_config(hellcreek, port, &config);
	if (err)
		return err;

	/* Save the chosen configuration to be returned later */
	memcpy(&ps->tstamp_config, &config, sizeof(config));

	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
		-EFAULT : 0;
}

int hellcreek_port_hwtstamp_get(struct dsa_switch *ds, int port,
				struct ifreq *ifr)
{
	struct hellcreek *hellcreek = ds->priv;
	struct hellcreek_port_hwtstamp *ps;
	struct hwtstamp_config *config;

	ps = &hellcreek->ports[port].port_hwtstamp;
	config = &ps->tstamp_config;

	return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
		-EFAULT : 0;
}

/* Returns a pointer to the PTP header if the caller should time stamp, or NULL
 * if the caller should not.
 */
static struct ptp_header *hellcreek_should_tstamp(struct hellcreek *hellcreek,
						  int port, struct sk_buff *skb,
						  unsigned int type)
{
	struct hellcreek_port_hwtstamp *ps =
		&hellcreek->ports[port].port_hwtstamp;
	struct ptp_header *hdr;

	hdr = ptp_parse_header(skb, type);
	if (!hdr)
		return NULL;

	if (!test_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state))
		return NULL;

	return hdr;
}

static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr)
{
	return be32_to_cpu(hdr->reserved2);
}

static void hellcreek_clear_reserved_field(struct ptp_header *hdr)
{
	hdr->reserved2 = 0;
}

static int hellcreek_ptp_hwtstamp_available(struct hellcreek *hellcreek,
					    unsigned int ts_reg)
{
	u16 status;

	status = hellcreek_ptp_read(hellcreek, ts_reg);

	if (status & PR_TS_STATUS_TS_LOST)
		dev_err(hellcreek->dev,
			"Tx time stamp lost! This should never happen!\n");

	/* If hwtstamp is not available, this means the previous hwtstamp was
	 * successfully read, and the one we need is not yet available
	 */
	return (status & PR_TS_STATUS_TS_AVAIL) ? 1 : 0;
}

/* Get nanoseconds timestamp from timestamping unit */
static u64 hellcreek_ptp_hwtstamp_read(struct hellcreek *hellcreek,
				       unsigned int ts_reg)
{
	u16 nsl, nsh;

	nsh = hellcreek_ptp_read(hellcreek, ts_reg);
	nsh = hellcreek_ptp_read(hellcreek, ts_reg);
	nsh = hellcreek_ptp_read(hellcreek, ts_reg);
	nsh = hellcreek_ptp_read(hellcreek, ts_reg);
	nsl = hellcreek_ptp_read(hellcreek, ts_reg);

	return (u64)nsl | ((u64)nsh << 16);
}

static int hellcreek_txtstamp_work(struct hellcreek *hellcreek,
				   struct hellcreek_port_hwtstamp *ps, int port)
{
	struct skb_shared_hwtstamps shhwtstamps;
	unsigned int status_reg, data_reg;
	struct sk_buff *tmp_skb;
	int ts_status;
	u64 ns = 0;

	if (!ps->tx_skb)
		return 0;

	switch (port) {
	case 2:
		status_reg = PR_TS_TX_P1_STATUS_C;
		data_reg   = PR_TS_TX_P1_DATA_C;
		break;
	case 3:
		status_reg = PR_TS_TX_P2_STATUS_C;
		data_reg   = PR_TS_TX_P2_DATA_C;
		break;
	default:
		dev_err(hellcreek->dev, "Wrong port for timestamping!\n");
		return 0;
	}

	ts_status = hellcreek_ptp_hwtstamp_available(hellcreek, status_reg);

	/* Not available yet? */
	if (ts_status == 0) {
		/* Check whether the operation of reading the tx timestamp has
		 * exceeded its allowed period
		 */
		if (time_is_before_jiffies(ps->tx_tstamp_start +
					   TX_TSTAMP_TIMEOUT)) {
			dev_err(hellcreek->dev,
				"Timeout while waiting for Tx timestamp!\n");
			goto free_and_clear_skb;
		}

		/* The timestamp should be available quickly, while getting it
		 * in high priority. Restart the work
		 */
		return 1;
	}

	mutex_lock(&hellcreek->ptp_lock);
	ns  = hellcreek_ptp_hwtstamp_read(hellcreek, data_reg);
	ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
	mutex_unlock(&hellcreek->ptp_lock);

	/* Now we have the timestamp in nanoseconds, store it in the correct
	 * structure in order to send it to the user
	 */
	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
	shhwtstamps.hwtstamp = ns_to_ktime(ns);

	tmp_skb = ps->tx_skb;
	ps->tx_skb = NULL;

	/* skb_complete_tx_timestamp() frees up the client to make another
	 * timestampable transmit.  We have to be ready for it by clearing the
	 * ps->tx_skb "flag" beforehand
	 */
	clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);

	/* Deliver a clone of the original outgoing tx_skb with tx hwtstamp */
	skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);

	return 0;

free_and_clear_skb:
	dev_kfree_skb_any(ps->tx_skb);
	ps->tx_skb = NULL;
	clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);

	return 0;
}

static void hellcreek_get_rxts(struct hellcreek *hellcreek,
			       struct hellcreek_port_hwtstamp *ps,
			       struct sk_buff *skb, struct sk_buff_head *rxq,
			       int port)
{
	struct skb_shared_hwtstamps *shwt;
	struct sk_buff_head received;
	unsigned long flags;

	/* The latched timestamp belongs to one of the received frames. */
	__skb_queue_head_init(&received);

	/* Lock & disable interrupts */
	spin_lock_irqsave(&rxq->lock, flags);

	/* Add the reception queue "rxq" to the "received" queue an reintialize
	 * "rxq".  From now on, we deal with "received" not with "rxq"
	 */
	skb_queue_splice_tail_init(rxq, &received);

	spin_unlock_irqrestore(&rxq->lock, flags);

	for (; skb; skb = __skb_dequeue(&received)) {
		struct ptp_header *hdr;
		unsigned int type;
		u64 ns;

		/* Get nanoseconds from ptp packet */
		type = SKB_PTP_TYPE(skb);
		hdr  = ptp_parse_header(skb, type);
		ns   = hellcreek_get_reserved_field(hdr);
		hellcreek_clear_reserved_field(hdr);

		/* Add seconds part */
		mutex_lock(&hellcreek->ptp_lock);
		ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
		mutex_unlock(&hellcreek->ptp_lock);

		/* Save time stamp */
		shwt = skb_hwtstamps(skb);
		memset(shwt, 0, sizeof(*shwt));
		shwt->hwtstamp = ns_to_ktime(ns);
		netif_rx_ni(skb);
	}
}

static void hellcreek_rxtstamp_work(struct hellcreek *hellcreek,
				    struct hellcreek_port_hwtstamp *ps,
				    int port)
{
	struct sk_buff *skb;

	skb = skb_dequeue(&ps->rx_queue);
	if (skb)
		hellcreek_get_rxts(hellcreek, ps, skb, &ps->rx_queue, port);
}

long hellcreek_hwtstamp_work(struct ptp_clock_info *ptp)
{
	struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
	struct dsa_switch *ds = hellcreek->ds;
	int i, restart = 0;

	for (i = 0; i < ds->num_ports; i++) {
		struct hellcreek_port_hwtstamp *ps;

		if (!dsa_is_user_port(ds, i))
			continue;

		ps = &hellcreek->ports[i].port_hwtstamp;

		if (test_bit(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
			restart |= hellcreek_txtstamp_work(hellcreek, ps, i);

		hellcreek_rxtstamp_work(hellcreek, ps, i);
	}

	return restart ? 1 : -1;
}

bool hellcreek_port_txtstamp(struct dsa_switch *ds, int port,
			     struct sk_buff *clone, unsigned int type)
{
	struct hellcreek *hellcreek = ds->priv;
	struct hellcreek_port_hwtstamp *ps;
	struct ptp_header *hdr;

	ps = &hellcreek->ports[port].port_hwtstamp;

	/* Check if the driver is expected to do HW timestamping */
	if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
		return false;

	/* Make sure the message is a PTP message that needs to be timestamped
	 * and the interaction with the HW timestamping is enabled. If not, stop
	 * here
	 */
	hdr = hellcreek_should_tstamp(hellcreek, port, clone, type);
	if (!hdr)
		return false;

	if (test_and_set_bit_lock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS,
				  &ps->state))
		return false;

	ps->tx_skb = clone;

	/* store the number of ticks occurred since system start-up till this
	 * moment
	 */
	ps->tx_tstamp_start = jiffies;

	ptp_schedule_worker(hellcreek->ptp_clock, 0);

	return true;
}

bool hellcreek_port_rxtstamp(struct dsa_switch *ds, int port,
			     struct sk_buff *skb, unsigned int type)
{
	struct hellcreek *hellcreek = ds->priv;
	struct hellcreek_port_hwtstamp *ps;
	struct ptp_header *hdr;

	ps = &hellcreek->ports[port].port_hwtstamp;

	/* This check only fails if the user did not initialize hardware
	 * timestamping beforehand.
	 */
	if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
		return false;

	/* Make sure the message is a PTP message that needs to be timestamped
	 * and the interaction with the HW timestamping is enabled. If not, stop
	 * here
	 */
	hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
	if (!hdr)
		return false;

	SKB_PTP_TYPE(skb) = type;

	skb_queue_tail(&ps->rx_queue, skb);

	ptp_schedule_worker(hellcreek->ptp_clock, 0);

	return true;
}

static void hellcreek_hwtstamp_port_setup(struct hellcreek *hellcreek, int port)
{
	struct hellcreek_port_hwtstamp *ps =
		&hellcreek->ports[port].port_hwtstamp;

	skb_queue_head_init(&ps->rx_queue);
}

int hellcreek_hwtstamp_setup(struct hellcreek *hellcreek)
{
	struct dsa_switch *ds = hellcreek->ds;
	int i;

	/* Initialize timestamping ports. */
	for (i = 0; i < ds->num_ports; ++i) {
		if (!dsa_is_user_port(ds, i))
			continue;

		hellcreek_hwtstamp_port_setup(hellcreek, i);
	}

	/* Select the synchronized clock as the source timekeeper for the
	 * timestamps and enable inline timestamping.
	 */
	hellcreek_ptp_write(hellcreek, PR_SETTINGS_C_TS_SRC_TK_MASK |
			    PR_SETTINGS_C_RES3TS,
			    PR_SETTINGS_C);

	return 0;
}

void hellcreek_hwtstamp_free(struct hellcreek *hellcreek)
{
	/* Nothing todo */
}