linux/drivers/net/dsa/microchip/ksz_ptp.c

// SPDX-License-Identifier: GPL-2.0
/* Microchip KSZ PTP Implementation
 *
 * Copyright (C) 2020 ARRI Lighting
 * Copyright (C) 2022 Microchip Technology Inc.
 */

#include <linux/dsa/ksz_common.h>
#include <linux/irq.h>
#include <linux/irqdomain.h>
#include <linux/kernel.h>
#include <linux/ptp_classify.h>
#include <linux/ptp_clock_kernel.h>

#include "ksz_common.h"
#include "ksz_ptp.h"
#include "ksz_ptp_reg.h"

#define ptp_caps_to_data(d) container_of((d), struct ksz_ptp_data, caps)
#define ptp_data_to_ksz_dev(d) container_of((d), struct ksz_device, ptp_data)

/* Sub-nanoseconds-adj,max * sub-nanoseconds / 40ns * 1ns
 * = (2^30-1) * (2 ^ 32) / 40 ns * 1 ns = 6249999
 */
#define KSZ_MAX_DRIFT_CORR 6249999

#define KSZ_PTP_INC_NS 40ULL  /* HW clock is incremented every 40 ns (by 40) */
#define KSZ_PTP_SUBNS_BITS 32

#define KSZ_PTP_INT_START 13

static int ksz_ptp_enable_mode(struct ksz_device *dev)
{
	struct ksz_tagger_data *tagger_data = ksz_tagger_data(dev->ds);
	struct ksz_ptp_data *ptp_data = &dev->ptp_data;
	struct ksz_port *prt;
	struct dsa_port *dp;
	bool tag_en = false;
	int ret;

	dsa_switch_for_each_user_port(dp, dev->ds) {
		prt = &dev->ports[dp->index];
		if (prt->hwts_tx_en || prt->hwts_rx_en) {
			tag_en = true;
			break;
		}
	}

	if (tag_en) {
		ret = ptp_schedule_worker(ptp_data->clock, 0);
		if (ret)
			return ret;
	} else {
		ptp_cancel_worker_sync(ptp_data->clock);
	}

	tagger_data->hwtstamp_set_state(dev->ds, tag_en);

	return ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_ENABLE,
			 tag_en ? PTP_ENABLE : 0);
}

/* The function is return back the capability of timestamping feature when
 * requested through ethtool -T <interface> utility
 */
int ksz_get_ts_info(struct dsa_switch *ds, int port, struct ethtool_ts_info *ts)
{
	struct ksz_device *dev = ds->priv;
	struct ksz_ptp_data *ptp_data;

	ptp_data = &dev->ptp_data;

	if (!ptp_data->clock)
		return -ENODEV;

	ts->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
			      SOF_TIMESTAMPING_RX_HARDWARE |
			      SOF_TIMESTAMPING_RAW_HARDWARE;

	ts->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ONESTEP_P2P);

	ts->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
			 BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
			 BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
			 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);

	ts->phc_index = ptp_clock_index(ptp_data->clock);

	return 0;
}

int ksz_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr)
{
	struct ksz_device *dev = ds->priv;
	struct hwtstamp_config *config;
	struct ksz_port *prt;

	prt = &dev->ports[port];
	config = &prt->tstamp_config;

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

static int ksz_set_hwtstamp_config(struct ksz_device *dev,
				   struct ksz_port *prt,
				   struct hwtstamp_config *config)
{
	if (config->flags)
		return -EINVAL;

	switch (config->tx_type) {
	case HWTSTAMP_TX_OFF:
		prt->hwts_tx_en = false;
		break;
	case HWTSTAMP_TX_ONESTEP_P2P:
		prt->hwts_tx_en = true;
		break;
	default:
		return -ERANGE;
	}

	switch (config->rx_filter) {
	case HWTSTAMP_FILTER_NONE:
		prt->hwts_rx_en = false;
		break;
	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
		prt->hwts_rx_en = true;
		break;
	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
		prt->hwts_rx_en = true;
		break;
	case HWTSTAMP_FILTER_PTP_V2_EVENT:
	case HWTSTAMP_FILTER_PTP_V2_SYNC:
		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
		prt->hwts_rx_en = true;
		break;
	default:
		config->rx_filter = HWTSTAMP_FILTER_NONE;
		return -ERANGE;
	}

	return ksz_ptp_enable_mode(dev);
}

int ksz_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
{
	struct ksz_device *dev = ds->priv;
	struct hwtstamp_config config;
	struct ksz_port *prt;
	int ret;

	prt = &dev->ports[port];

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

	ret = ksz_set_hwtstamp_config(dev, prt, &config);
	if (ret)
		return ret;

	memcpy(&prt->tstamp_config, &config, sizeof(config));

	return copy_to_user(ifr->ifr_data, &config, sizeof(config));
}

static ktime_t ksz_tstamp_reconstruct(struct ksz_device *dev, ktime_t tstamp)
{
	struct timespec64 ptp_clock_time;
	struct ksz_ptp_data *ptp_data;
	struct timespec64 diff;
	struct timespec64 ts;

	ptp_data = &dev->ptp_data;
	ts = ktime_to_timespec64(tstamp);

	spin_lock_bh(&ptp_data->clock_lock);
	ptp_clock_time = ptp_data->clock_time;
	spin_unlock_bh(&ptp_data->clock_lock);

	/* calculate full time from partial time stamp */
	ts.tv_sec = (ptp_clock_time.tv_sec & ~3) | ts.tv_sec;

	/* find nearest possible point in time */
	diff = timespec64_sub(ts, ptp_clock_time);
	if (diff.tv_sec > 2)
		ts.tv_sec -= 4;
	else if (diff.tv_sec < -2)
		ts.tv_sec += 4;

	return timespec64_to_ktime(ts);
}

bool ksz_port_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb,
		       unsigned int type)
{
	struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);
	struct ksz_device *dev = ds->priv;
	struct ptp_header *ptp_hdr;
	u8 ptp_msg_type;
	ktime_t tstamp;
	s64 correction;

	tstamp = KSZ_SKB_CB(skb)->tstamp;
	memset(hwtstamps, 0, sizeof(*hwtstamps));
	hwtstamps->hwtstamp = ksz_tstamp_reconstruct(dev, tstamp);

	ptp_hdr = ptp_parse_header(skb, type);
	if (!ptp_hdr)
		goto out;

	ptp_msg_type = ptp_get_msgtype(ptp_hdr, type);
	if (ptp_msg_type != PTP_MSGTYPE_PDELAY_REQ)
		goto out;

	/* Only subtract the partial time stamp from the correction field.  When
	 * the hardware adds the egress time stamp to the correction field of
	 * the PDelay_Resp message on tx, also only the partial time stamp will
	 * be added.
	 */
	correction = (s64)get_unaligned_be64(&ptp_hdr->correction);
	correction -= ktime_to_ns(tstamp) << 16;

	ptp_header_update_correction(skb, type, ptp_hdr, correction);

out:
	return false;
}

static int _ksz_ptp_gettime(struct ksz_device *dev, struct timespec64 *ts)
{
	u32 nanoseconds;
	u32 seconds;
	u8 phase;
	int ret;

	/* Copy current PTP clock into shadow registers and read */
	ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_READ_TIME, PTP_READ_TIME);
	if (ret)
		return ret;

	ret = ksz_read8(dev, REG_PTP_RTC_SUB_NANOSEC__2, &phase);
	if (ret)
		return ret;

	ret = ksz_read32(dev, REG_PTP_RTC_NANOSEC, &nanoseconds);
	if (ret)
		return ret;

	ret = ksz_read32(dev, REG_PTP_RTC_SEC, &seconds);
	if (ret)
		return ret;

	ts->tv_sec = seconds;
	ts->tv_nsec = nanoseconds + phase * 8;

	return 0;
}

static int ksz_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
{
	struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp);
	struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data);
	int ret;

	mutex_lock(&ptp_data->lock);
	ret = _ksz_ptp_gettime(dev, ts);
	mutex_unlock(&ptp_data->lock);

	return ret;
}

static int ksz_ptp_settime(struct ptp_clock_info *ptp,
			   const struct timespec64 *ts)
{
	struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp);
	struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data);
	int ret;

	mutex_lock(&ptp_data->lock);

	/* Write to shadow registers and Load PTP clock */
	ret = ksz_write16(dev, REG_PTP_RTC_SUB_NANOSEC__2, PTP_RTC_0NS);
	if (ret)
		goto unlock;

	ret = ksz_write32(dev, REG_PTP_RTC_NANOSEC, ts->tv_nsec);
	if (ret)
		goto unlock;

	ret = ksz_write32(dev, REG_PTP_RTC_SEC, ts->tv_sec);
	if (ret)
		goto unlock;

	ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_LOAD_TIME, PTP_LOAD_TIME);
	if (ret)
		goto unlock;

	spin_lock_bh(&ptp_data->clock_lock);
	ptp_data->clock_time = *ts;
	spin_unlock_bh(&ptp_data->clock_lock);

unlock:
	mutex_unlock(&ptp_data->lock);

	return ret;
}

static int ksz_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
{
	struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp);
	struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data);
	u64 base, adj;
	bool negative;
	u32 data32;
	int ret;

	mutex_lock(&ptp_data->lock);

	if (scaled_ppm) {
		base = KSZ_PTP_INC_NS << KSZ_PTP_SUBNS_BITS;
		negative = diff_by_scaled_ppm(base, scaled_ppm, &adj);

		data32 = (u32)adj;
		data32 &= PTP_SUBNANOSEC_M;
		if (!negative)
			data32 |= PTP_RATE_DIR;

		ret = ksz_write32(dev, REG_PTP_SUBNANOSEC_RATE, data32);
		if (ret)
			goto unlock;

		ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ADJ_ENABLE,
				PTP_CLK_ADJ_ENABLE);
		if (ret)
			goto unlock;
	} else {
		ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ADJ_ENABLE, 0);
		if (ret)
			goto unlock;
	}

unlock:
	mutex_unlock(&ptp_data->lock);
	return ret;
}

static int ksz_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
{
	struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp);
	struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data);
	struct timespec64 delta64 = ns_to_timespec64(delta);
	s32 sec, nsec;
	u16 data16;
	int ret;

	mutex_lock(&ptp_data->lock);

	/* do not use ns_to_timespec64(),
	 * both sec and nsec are subtracted by hw
	 */
	sec = div_s64_rem(delta, NSEC_PER_SEC, &nsec);

	ret = ksz_write32(dev, REG_PTP_RTC_NANOSEC, abs(nsec));
	if (ret)
		goto unlock;

	ret = ksz_write32(dev, REG_PTP_RTC_SEC, abs(sec));
	if (ret)
		goto unlock;

	ret = ksz_read16(dev, REG_PTP_CLK_CTRL, &data16);
	if (ret)
		goto unlock;

	data16 |= PTP_STEP_ADJ;

	/* PTP_STEP_DIR -- 0: subtract, 1: add */
	if (delta < 0)
		data16 &= ~PTP_STEP_DIR;
	else
		data16 |= PTP_STEP_DIR;

	ret = ksz_write16(dev, REG_PTP_CLK_CTRL, data16);
	if (ret)
		goto unlock;

	spin_lock_bh(&ptp_data->clock_lock);
	ptp_data->clock_time = timespec64_add(ptp_data->clock_time, delta64);
	spin_unlock_bh(&ptp_data->clock_lock);

unlock:
	mutex_unlock(&ptp_data->lock);
	return ret;
}

/*  Function is pointer to the do_aux_work in the ptp_clock capability */
static long ksz_ptp_do_aux_work(struct ptp_clock_info *ptp)
{
	struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp);
	struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data);
	struct timespec64 ts;
	int ret;

	mutex_lock(&ptp_data->lock);
	ret = _ksz_ptp_gettime(dev, &ts);
	if (ret)
		goto out;

	spin_lock_bh(&ptp_data->clock_lock);
	ptp_data->clock_time = ts;
	spin_unlock_bh(&ptp_data->clock_lock);

out:
	mutex_unlock(&ptp_data->lock);

	return HZ;  /* reschedule in 1 second */
}

static int ksz_ptp_start_clock(struct ksz_device *dev)
{
	struct ksz_ptp_data *ptp_data = &dev->ptp_data;
	int ret;

	ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ENABLE, PTP_CLK_ENABLE);
	if (ret)
		return ret;

	ptp_data->clock_time.tv_sec = 0;
	ptp_data->clock_time.tv_nsec = 0;

	return 0;
}

int ksz_ptp_clock_register(struct dsa_switch *ds)
{
	struct ksz_device *dev = ds->priv;
	struct ksz_ptp_data *ptp_data;
	int ret;

	ptp_data = &dev->ptp_data;
	mutex_init(&ptp_data->lock);
	spin_lock_init(&ptp_data->clock_lock);

	ptp_data->caps.owner		= THIS_MODULE;
	snprintf(ptp_data->caps.name, 16, "Microchip Clock");
	ptp_data->caps.max_adj		= KSZ_MAX_DRIFT_CORR;
	ptp_data->caps.gettime64	= ksz_ptp_gettime;
	ptp_data->caps.settime64	= ksz_ptp_settime;
	ptp_data->caps.adjfine		= ksz_ptp_adjfine;
	ptp_data->caps.adjtime		= ksz_ptp_adjtime;
	ptp_data->caps.do_aux_work	= ksz_ptp_do_aux_work;

	ret = ksz_ptp_start_clock(dev);
	if (ret)
		return ret;

	/* Currently only P2P mode is supported. When 802_1AS bit is set, it
	 * forwards all PTP packets to host port and none to other ports.
	 */
	ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_TC_P2P | PTP_802_1AS,
			PTP_TC_P2P | PTP_802_1AS);
	if (ret)
		return ret;

	ptp_data->clock = ptp_clock_register(&ptp_data->caps, dev->dev);
	if (IS_ERR_OR_NULL(ptp_data->clock))
		return PTR_ERR(ptp_data->clock);

	return 0;
}

void ksz_ptp_clock_unregister(struct dsa_switch *ds)
{
	struct ksz_device *dev = ds->priv;
	struct ksz_ptp_data *ptp_data;

	ptp_data = &dev->ptp_data;

	if (ptp_data->clock)
		ptp_clock_unregister(ptp_data->clock);
}

static irqreturn_t ksz_ptp_msg_thread_fn(int irq, void *dev_id)
{
	return IRQ_NONE;
}

static irqreturn_t ksz_ptp_irq_thread_fn(int irq, void *dev_id)
{
	struct ksz_irq *ptpirq = dev_id;
	unsigned int nhandled = 0;
	struct ksz_device *dev;
	unsigned int sub_irq;
	u16 data;
	int ret;
	u8 n;

	dev = ptpirq->dev;

	ret = ksz_read16(dev, ptpirq->reg_status, &data);
	if (ret)
		goto out;

	/* Clear the interrupts W1C */
	ret = ksz_write16(dev, ptpirq->reg_status, data);
	if (ret)
		return IRQ_NONE;

	for (n = 0; n < ptpirq->nirqs; ++n) {
		if (data & BIT(n + KSZ_PTP_INT_START)) {
			sub_irq = irq_find_mapping(ptpirq->domain, n);
			handle_nested_irq(sub_irq);
			++nhandled;
		}
	}

out:
	return (nhandled > 0 ? IRQ_HANDLED : IRQ_NONE);
}

static void ksz_ptp_irq_mask(struct irq_data *d)
{
	struct ksz_irq *kirq = irq_data_get_irq_chip_data(d);

	kirq->masked &= ~BIT(d->hwirq + KSZ_PTP_INT_START);
}

static void ksz_ptp_irq_unmask(struct irq_data *d)
{
	struct ksz_irq *kirq = irq_data_get_irq_chip_data(d);

	kirq->masked |= BIT(d->hwirq + KSZ_PTP_INT_START);
}

static void ksz_ptp_irq_bus_lock(struct irq_data *d)
{
	struct ksz_irq *kirq  = irq_data_get_irq_chip_data(d);

	mutex_lock(&kirq->dev->lock_irq);
}

static void ksz_ptp_irq_bus_sync_unlock(struct irq_data *d)
{
	struct ksz_irq *kirq  = irq_data_get_irq_chip_data(d);
	struct ksz_device *dev = kirq->dev;
	int ret;

	ret = ksz_write16(dev, kirq->reg_mask, kirq->masked);
	if (ret)
		dev_err(dev->dev, "failed to change IRQ mask\n");

	mutex_unlock(&dev->lock_irq);
}

static const struct irq_chip ksz_ptp_irq_chip = {
	.name			= "ksz-irq",
	.irq_mask		= ksz_ptp_irq_mask,
	.irq_unmask		= ksz_ptp_irq_unmask,
	.irq_bus_lock		= ksz_ptp_irq_bus_lock,
	.irq_bus_sync_unlock	= ksz_ptp_irq_bus_sync_unlock,
};

static int ksz_ptp_irq_domain_map(struct irq_domain *d,
				  unsigned int irq, irq_hw_number_t hwirq)
{
	irq_set_chip_data(irq, d->host_data);
	irq_set_chip_and_handler(irq, &ksz_ptp_irq_chip, handle_level_irq);
	irq_set_noprobe(irq);

	return 0;
}

static const struct irq_domain_ops ksz_ptp_irq_domain_ops = {
	.map	= ksz_ptp_irq_domain_map,
	.xlate	= irq_domain_xlate_twocell,
};

static void ksz_ptp_msg_irq_free(struct ksz_port *port, u8 n)
{
	struct ksz_ptp_irq *ptpmsg_irq;

	ptpmsg_irq = &port->ptpmsg_irq[n];

	free_irq(ptpmsg_irq->num, ptpmsg_irq);
	irq_dispose_mapping(ptpmsg_irq->num);
}

static int ksz_ptp_msg_irq_setup(struct ksz_port *port, u8 n)
{
	u16 ts_reg[] = {REG_PTP_PORT_PDRESP_TS, REG_PTP_PORT_XDELAY_TS,
			REG_PTP_PORT_SYNC_TS};
	static const char * const name[] = {"pdresp-msg", "xdreq-msg",
					    "sync-msg"};
	const struct ksz_dev_ops *ops = port->ksz_dev->dev_ops;
	struct ksz_ptp_irq *ptpmsg_irq;

	ptpmsg_irq = &port->ptpmsg_irq[n];

	ptpmsg_irq->port = port;
	ptpmsg_irq->ts_reg = ops->get_port_addr(port->num, ts_reg[n]);

	snprintf(ptpmsg_irq->name, sizeof(ptpmsg_irq->name), name[n]);

	ptpmsg_irq->num = irq_find_mapping(port->ptpirq.domain, n);
	if (ptpmsg_irq->num < 0)
		return ptpmsg_irq->num;

	return request_threaded_irq(ptpmsg_irq->num, NULL,
				    ksz_ptp_msg_thread_fn, IRQF_ONESHOT,
				    ptpmsg_irq->name, ptpmsg_irq);
}

int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p)
{
	struct ksz_device *dev = ds->priv;
	const struct ksz_dev_ops *ops = dev->dev_ops;
	struct ksz_port *port = &dev->ports[p];
	struct ksz_irq *ptpirq = &port->ptpirq;
	int irq;
	int ret;

	ptpirq->dev = dev;
	ptpirq->masked = 0;
	ptpirq->nirqs = 3;
	ptpirq->reg_mask = ops->get_port_addr(p, REG_PTP_PORT_TX_INT_ENABLE__2);
	ptpirq->reg_status = ops->get_port_addr(p,
						REG_PTP_PORT_TX_INT_STATUS__2);
	snprintf(ptpirq->name, sizeof(ptpirq->name), "ptp-irq-%d", p);

	ptpirq->domain = irq_domain_add_linear(dev->dev->of_node, ptpirq->nirqs,
					       &ksz_ptp_irq_domain_ops, ptpirq);
	if (!ptpirq->domain)
		return -ENOMEM;

	for (irq = 0; irq < ptpirq->nirqs; irq++)
		irq_create_mapping(ptpirq->domain, irq);

	ptpirq->irq_num = irq_find_mapping(port->pirq.domain, PORT_SRC_PTP_INT);
	if (ptpirq->irq_num < 0) {
		ret = ptpirq->irq_num;
		goto out;
	}

	ret = request_threaded_irq(ptpirq->irq_num, NULL, ksz_ptp_irq_thread_fn,
				   IRQF_ONESHOT, ptpirq->name, ptpirq);
	if (ret)
		goto out;

	for (irq = 0; irq < ptpirq->nirqs; irq++) {
		ret = ksz_ptp_msg_irq_setup(port, irq);
		if (ret)
			goto out_ptp_msg;
	}

	return 0;

out_ptp_msg:
	free_irq(ptpirq->irq_num, ptpirq);
	while (irq--)
		free_irq(port->ptpmsg_irq[irq].num, &port->ptpmsg_irq[irq]);
out:
	for (irq = 0; irq < ptpirq->nirqs; irq++)
		irq_dispose_mapping(port->ptpmsg_irq[irq].num);

	irq_domain_remove(ptpirq->domain);

	return ret;
}

void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p)
{
	struct ksz_device *dev = ds->priv;
	struct ksz_port *port = &dev->ports[p];
	struct ksz_irq *ptpirq = &port->ptpirq;
	u8 n;

	for (n = 0; n < ptpirq->nirqs; n++)
		ksz_ptp_msg_irq_free(port, n);

	free_irq(ptpirq->irq_num, ptpirq);
	irq_dispose_mapping(ptpirq->irq_num);

	irq_domain_remove(ptpirq->domain);
}

MODULE_AUTHOR("Christian Eggers <ceggers@arri.de>");
MODULE_AUTHOR("Arun Ramadoss <arun.ramadoss@microchip.com>");
MODULE_DESCRIPTION("PTP support for KSZ switch");
MODULE_LICENSE("GPL");