[PATCH] can: xilinx_can: add support for RX timestamps on Zynq

[PATCH] can: xilinx_can: add support for RX timestamps on Zynq

[Matej Vasilevski]

This patch adds support for hardware RX timestamps from Xilinx Zynq CAN
controllers. The timestamp is calculated against a timepoint reference
stored when the first CAN message is received.

When CAN bus traffic does not contain long idle pauses (so that
the clocks would drift by a multiple of the counter rollover time),
then the hardware timestamps provide precise relative time between
received messages. This can be used e.g. for latency testing.

Co-authored-by: Martin Jerabek <martin.jerabek01@gmail.com>
Signed-off-by: Matej Vasilevski <matej.vasilevski@seznam.cz>
---
 drivers/net/can/xilinx_can.c | 171 ++++++++++++++++++++++++++++++++++-
 1 file changed, 168 insertions(+), 3 deletions(-)

diff --git a/drivers/net/can/xilinx_can.c b/drivers/net/can/xilinx_can.c
index 393b2d9f9d2a..12e3ca856684 100644
--- a/drivers/net/can/xilinx_can.c
+++ b/drivers/net/can/xilinx_can.c
@@ -11,11 +11,14 @@
 
 #include <linux/bitfield.h>
 #include <linux/clk.h>
+#include <linux/clocksource.h>
 #include <linux/errno.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
+#include "linux/ktime.h"
+#include "linux/math64.h"
 #include <linux/module.h>
 #include <linux/netdevice.h>
 #include <linux/of.h>
@@ -165,6 +168,10 @@ enum xcan_reg {
 #define XCAN_FLAG_RX_FIFO_MULTI	0x0010
 #define XCAN_FLAG_CANFD_2	0x0020
 
+#define XCAN_ZYNQ_TSTAMP_BITS	16
+#define XCAN_ZYNQ_TSTAMP_BIT_MASK	GENMASK(15, 0)
+#define XCAN_ZYNQ_TSTAMP_MAX_VAL	BIT(XCAN_ZYNQ_TSTAMP_BITS)
+
 enum xcan_ip_type {
 	XAXI_CAN = 0,
 	XZYNQ_CANPS,
@@ -181,6 +188,11 @@ struct xcan_devtype_data {
 	unsigned int btr_sjw_shift;
 };
 
+struct xcan_timepoint {
+	u64 ktime_ns;
+	u16 ts;
+};
+
 /**
  * struct xcan_priv - This definition define CAN driver instance
  * @can:			CAN private data structure.
@@ -197,6 +209,11 @@ struct xcan_devtype_data {
  * @bus_clk:			Pointer to struct clk
  * @can_clk:			Pointer to struct clk
  * @devtype:			Device type specific constants
+ * @ref_timepoint:		Reference timepoint for synchronizing ktime with HW ticks
+ * @tstamp_rollover_ns:		Timestamping counter rollover time in nanoseconds
+ * @cantime2ns_mul:		Mult. constant for converting from counter time to nanoseconds
+ * @cantime2ns_shr:		Right shift for converting from counter time to nanoseconds
+ * @rx_timestamps_enabled:	Boolean flag indicating whether rx timestamps should be computed
  */
 struct xcan_priv {
 	struct can_priv can;
@@ -214,6 +231,11 @@ struct xcan_priv {
 	struct clk *bus_clk;
 	struct clk *can_clk;
 	struct xcan_devtype_data devtype;
+	struct xcan_timepoint ref_timepoint;
+	u64 tstamp_rollover_ns;
+	u32 cantime2ns_mul;
+	u32 cantime2ns_shr;
+	bool rx_timestamps_enabled;
 };
 
 /* CAN Bittiming constants as per Xilinx CAN specs */
@@ -759,6 +781,50 @@ static netdev_tx_t xcan_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	return NETDEV_TX_OK;
 }
 
+/**
+ * xcan_timestamp_to_ktime - Converts the hardware timestamp to ktime timestamp
+ * @priv:	Driver private data structure
+ * @ts:		Timestamp from the hardware counter
+ *
+ * Calculating the frame ktime using rollover count and initial timepoint
+ * reference allows to have constant delay between the moment the frame is
+ * timestamped in hardware, and the timestamp reported by the driver.
+ * When CAN bus traffic does not contain long idle pauses (so that the clocks
+ * would drift by a multiple of the counter rollover time), then timestamps
+ * provide precise relative time between received messages.
+ *
+ * Return: hardware timestamp in ktime
+ */
+static ktime_t xcan_timestamp_to_ktime(struct xcan_priv *priv, u16 ts)
+{
+	struct xcan_timepoint *ref = &priv->ref_timepoint;
+	u64 ktime_now_ns;
+	u64 ktime_diff_ns;
+	u16 tstamp_diff;
+	u64 tstamp_diff_ns;
+	u64 time_diff_ns;
+	u64 rollover_count;
+
+	ktime_now_ns = ktime_get_real_ns();
+	if (ref->ktime_ns == 0) {
+		/* first received frame */
+		ref->ktime_ns = ktime_now_ns;
+		ref->ts = ts;
+		return ns_to_ktime(ktime_now_ns);
+	}
+
+	tstamp_diff = ts > ref->ts ? ts - ref->ts : ref->ts - ts;
+	tstamp_diff_ns = mul_u32_u32((u32)tstamp_diff, priv->cantime2ns_mul)
+			 >> priv->cantime2ns_shr;
+	ktime_diff_ns = ktime_now_ns - ref->ktime_ns;
+	time_diff_ns = ktime_diff_ns - tstamp_diff_ns;
+	rollover_count = div_u64(time_diff_ns + (priv->tstamp_rollover_ns / 2),
+				 priv->tstamp_rollover_ns);
+
+	return ns_to_ktime(tstamp_diff_ns + rollover_count * priv->tstamp_rollover_ns)
+	       + ref->ktime_ns;
+}
+
 /**
  * xcan_rx -  Is called from CAN isr to complete the received
  *		frame  processing
@@ -776,7 +842,8 @@ static int xcan_rx(struct net_device *ndev, int frame_base)
 	struct net_device_stats *stats = &ndev->stats;
 	struct can_frame *cf;
 	struct sk_buff *skb;
-	u32 id_xcan, dlc, data[2] = {0, 0};
+	struct skb_shared_hwtstamps *hwts;
+	u32 id_xcan, dlc_reg, dlc, rawts, data[2] = {0, 0};
 
 	skb = alloc_can_skb(ndev, &cf);
 	if (unlikely(!skb)) {
@@ -786,12 +853,19 @@ static int xcan_rx(struct net_device *ndev, int frame_base)
 
 	/* Read a frame from Xilinx zynq CANPS */
 	id_xcan = priv->read_reg(priv, XCAN_FRAME_ID_OFFSET(frame_base));
-	dlc = priv->read_reg(priv, XCAN_FRAME_DLC_OFFSET(frame_base)) >>
-				   XCAN_DLCR_DLC_SHIFT;
+	dlc_reg = priv->read_reg(priv, XCAN_FRAME_DLC_OFFSET(frame_base));
+	dlc = dlc_reg >> XCAN_DLCR_DLC_SHIFT;
 
 	/* Change Xilinx CAN data length format to socketCAN data format */
 	cf->len = can_cc_dlc2len(dlc);
 
+	if (priv->rx_timestamps_enabled) {
+		hwts = skb_hwtstamps(skb);
+		memset(hwts, 0, sizeof(*hwts));
+		rawts = dlc_reg & XCAN_ZYNQ_TSTAMP_BIT_MASK;
+		hwts->hwtstamp = xcan_timestamp_to_ktime(priv, rawts);
+	}
+
 	/* Change Xilinx CAN ID format to socketCAN ID format */
 	if (id_xcan & XCAN_IDR_IDE_MASK) {
 		/* The received frame is an Extended format frame */
@@ -1532,11 +1606,95 @@ static int xcan_get_auto_tdcv(const struct net_device *ndev, u32 *tdcv)
 	return 0;
 }
 
+static int xcan_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
+{
+	struct xcan_priv *priv = netdev_priv(dev);
+	struct hwtstamp_config cfg;
+
+	if (priv->devtype.cantype != XZYNQ_CANPS)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
+		return -EFAULT;
+
+	if (cfg.flags)
+		return -EINVAL;
+
+	if (cfg.tx_type != HWTSTAMP_TX_OFF)
+		return -ERANGE;
+
+	switch (cfg.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		priv->rx_timestamps_enabled = false;
+		break;
+	case HWTSTAMP_FILTER_ALL:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+		fallthrough;
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+		priv->rx_timestamps_enabled = true;
+		cfg.rx_filter = HWTSTAMP_FILTER_ALL;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
+}
+
+static int xcan_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
+{
+	struct xcan_priv *priv = netdev_priv(dev);
+	struct hwtstamp_config cfg;
+
+	if (priv->devtype.cantype != XZYNQ_CANPS)
+		return -EOPNOTSUPP;
+
+	cfg.flags = 0;
+	cfg.tx_type = HWTSTAMP_TX_OFF;
+	cfg.rx_filter = priv->rx_timestamps_enabled ? HWTSTAMP_FILTER_ALL : HWTSTAMP_FILTER_NONE;
+
+	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
+}
+
+static int xcan_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	switch (cmd) {
+	case SIOCSHWTSTAMP:
+		return xcan_hwtstamp_set(dev, ifr);
+	case SIOCGHWTSTAMP:
+		return xcan_hwtstamp_get(dev, ifr);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 static const struct net_device_ops xcan_netdev_ops = {
 	.ndo_open	= xcan_open,
 	.ndo_stop	= xcan_close,
 	.ndo_start_xmit	= xcan_start_xmit,
 	.ndo_change_mtu	= can_change_mtu,
+	.ndo_eth_ioctl	= xcan_ioctl,
 };
 
 /**
@@ -1854,6 +2012,13 @@ static int xcan_probe(struct platform_device *pdev)
 
 	priv->can.clock.freq = clk_get_rate(priv->can_clk);
 
+	clocks_calc_mult_shift(&priv->cantime2ns_mul, &priv->cantime2ns_shr,
+			       priv->can.clock.freq, NSEC_PER_SEC, 0);
+	priv->rx_timestamps_enabled = false;
+	priv->tstamp_rollover_ns = mul_u32_u32((u32)XCAN_ZYNQ_TSTAMP_MAX_VAL, priv->cantime2ns_mul)
+				   >> priv->cantime2ns_shr;
+	memset(&priv->ref_timepoint, 0, sizeof(priv->ref_timepoint));
+
 	netif_napi_add_weight(ndev, &priv->napi, xcan_rx_poll, rx_max);
 
 	ret = register_candev(ndev);

base-commit: 874bdbfe624e577687c2053a26aab44715c68453
-- 
2.25.1

Re: [PATCH] can: xilinx_can: add support for RX timestamps on Zynq

[Marc Kleine-Budde]

[-- Attachment #1: Type: text/plain, Size: 1145 bytes --]

On 16.07.2022 14:04:09, Matej Vasilevski wrote:
> This patch adds support for hardware RX timestamps from Xilinx Zynq CAN
> controllers. The timestamp is calculated against a timepoint reference
> stored when the first CAN message is received.
> 
> When CAN bus traffic does not contain long idle pauses (so that
> the clocks would drift by a multiple of the counter rollover time),
> then the hardware timestamps provide precise relative time between
> received messages. This can be used e.g. for latency testing.

Please make use of the existing cyclecounter/timecounter framework. Is
there a way to read the current time from a register? If so, please
setup a worker that does that regularly.

Have a look at the mcp251xfd driver as an example:

https://elixir.bootlin.com/linux/latest/source/drivers/net/can/spi/mcp251xfd/mcp251xfd-timestamp.c

Marc

-- 
Pengutronix e.K.                 | Marc Kleine-Budde           |
Embedded Linux                   | https://www.pengutronix.de  |
Vertretung West/Dortmund         | Phone: +49-231-2826-924     |
Amtsgericht Hildesheim, HRA 2686 | Fax:   +49-5121-206917-5555 |

[-- Attachment #2: signature.asc --]
[-- Type: application/pgp-signature, Size: 488 bytes --]

Re: [PATCH] can: xilinx_can: add support for RX timestamps on Zynq

[Pavel Pisa]

Hello Marc,

On Monday 18 of July 2022 10:33:12 Marc Kleine-Budde wrote:
> On 16.07.2022 14:04:09, Matej Vasilevski wrote:
> > This patch adds support for hardware RX timestamps from Xilinx Zynq CAN
> > controllers. The timestamp is calculated against a timepoint reference
> > stored when the first CAN message is received.
> >
> > When CAN bus traffic does not contain long idle pauses (so that
> > the clocks would drift by a multiple of the counter rollover time),
> > then the hardware timestamps provide precise relative time between
> > received messages. This can be used e.g. for latency testing.
>
> Please make use of the existing cyclecounter/timecounter framework. Is
> there a way to read the current time from a register? If so, please
> setup a worker that does that regularly.
>
> Have a look at the mcp251xfd driver as an example:

Matej Vasilevski has looked at the example. But there is problem
that we know no method how to read actual counter value at least for
Xilinx Zynq 7000. May be we overlooked something or there
is hidden test register.

So actual support is the best approach we have found so far.
It is usable and valuable for precise relative time measurement
when bus is not idle for longer time. With expected clock
precision there should be no skip when at least one message
for each second or more is received.

The precision degrades to software software timer with
one half of timestamp counter period jitter for really
long gaps between messages. 

I understand that you do not like the situation,
if you think that it is not acceptable for mainline
even with config option under experimental then
never mind. We want to document this work on Linux
CAN mailing list. It worked for us in far past
when we used XCAN for CAN latency testing.

We have CTU CAN FD now which has in the default config
64 bits timestamps. It is readable and synchronized
(single counter) over all channels in our can latency
tester design for Zynq. 100 MHz timestamps base is
shared even over all CTU CAN FD cores when they
are integrated to PCIe card.

It could be intersting if XCAN or followups
on later Xilinx systems has additional registers
to read time base.

But I pose no UltraScale or later board at the
moment. I have organized the purchase of more
ones in 2016, but they stay in group which
break cooperation on the projects long time ago.

Best wishes,

                Pavel
-- 
                Pavel Pisa
    phone:      +420 603531357
    e-mail:     pisa@cmp.felk.cvut.cz
    Department of Control Engineering FEE CVUT
    Karlovo namesti 13, 121 35, Prague 2
    university: http://control.fel.cvut.cz/
    personal:   http://cmp.felk.cvut.cz/~pisa
    projects:   https://www.openhub.net/accounts/ppisa
    CAN related:http://canbus.pages.fel.cvut.cz/
    RISC-V education: https://comparch.edu.cvut.cz/
    Open Technologies Research Education and Exchange Services
    https://gitlab.fel.cvut.cz/otrees/org/-/wikis/home

Re: [PATCH] can: xilinx_can: add support for RX timestamps on Zynq

[Matej Vasilevski]

On Mon, Jul 18, 2022 at 10:33:12AM +0200, Marc Kleine-Budde wrote:
> On 16.07.2022 14:04:09, Matej Vasilevski wrote:
> > This patch adds support for hardware RX timestamps from Xilinx Zynq CAN
> > controllers. The timestamp is calculated against a timepoint reference
> > stored when the first CAN message is received.
> > 
> > When CAN bus traffic does not contain long idle pauses (so that
> > the clocks would drift by a multiple of the counter rollover time),
> > then the hardware timestamps provide precise relative time between
> > received messages. This can be used e.g. for latency testing.
> 
> Please make use of the existing cyclecounter/timecounter framework. Is
> there a way to read the current time from a register? If so, please
> setup a worker that does that regularly.
> 
> Have a look at the mcp251xfd driver as an example:
> 
> https://elixir.bootlin.com/linux/latest/source/drivers/net/can/spi/mcp251xfd/mcp251xfd-timestamp.c

Hi Marc,

as Pavel have said, the counter register isn't readable.

I'll try to fit the timecounter/cyclecounter framework and send a v2
patch if it works well. Thanks for the suggestion, it didn't occur to me
that I can use it in this case as well.

Regards,
Matej

Re: [PATCH] can: xilinx_can: add support for RX timestamps on Zynq

[Marc Kleine-Budde]

[-- Attachment #1: Type: text/plain, Size: 2060 bytes --]

I've added Michal Simek on into the loop.

Michal can you figure out if there's an undocumented register to read
the free running time stamp counter of the xilinx_can IP core?

On 18.07.2022 12:40:03, Matej Vasilevski wrote:
> On Mon, Jul 18, 2022 at 10:33:12AM +0200, Marc Kleine-Budde wrote:
> > On 16.07.2022 14:04:09, Matej Vasilevski wrote:
> > > This patch adds support for hardware RX timestamps from Xilinx Zynq CAN
> > > controllers. The timestamp is calculated against a timepoint reference
> > > stored when the first CAN message is received.
> > > 
> > > When CAN bus traffic does not contain long idle pauses (so that
> > > the clocks would drift by a multiple of the counter rollover time),
> > > then the hardware timestamps provide precise relative time between
> > > received messages. This can be used e.g. for latency testing.
> > 
> > Please make use of the existing cyclecounter/timecounter framework. Is
> > there a way to read the current time from a register? If so, please
> > setup a worker that does that regularly.
> > 
> > Have a look at the mcp251xfd driver as an example:
> > 
> > https://elixir.bootlin.com/linux/latest/source/drivers/net/can/spi/mcp251xfd/mcp251xfd-timestamp.c
> 
> Hi Marc,
> 
> as Pavel have said, the counter register isn't readable.

Some HW designer successfully ticked the mark [x] HW time stamp support!
But haven't thought on how to use this. No Overflow IRQ and no
possibility to read the free running timer. :/

> I'll try to fit the timecounter/cyclecounter framework and send a v2
> patch if it works well. Thanks for the suggestion, it didn't occur to me
> that I can use it in this case as well.

Maybe it's possible to add the ktimestamp based overflow calculation
there, too.

Marc

-- 
Pengutronix e.K.                 | Marc Kleine-Budde           |
Embedded Linux                   | https://www.pengutronix.de  |
Vertretung West/Dortmund         | Phone: +49-231-2826-924     |
Amtsgericht Hildesheim, HRA 2686 | Fax:   +49-5121-206917-5555 |

[-- Attachment #2: signature.asc --]
[-- Type: application/pgp-signature, Size: 488 bytes --]

Re: [PATCH] can: xilinx_can: add support for RX timestamps on Zynq

[Marc Kleine-Budde]

[-- Attachment #1: Type: text/plain, Size: 1606 bytes --]

On 18.07.2022 12:20:06, Pavel Pisa wrote:
> Hello Marc,
> 
> On Monday 18 of July 2022 10:33:12 Marc Kleine-Budde wrote:
> > On 16.07.2022 14:04:09, Matej Vasilevski wrote:
> > > This patch adds support for hardware RX timestamps from Xilinx Zynq CAN
> > > controllers. The timestamp is calculated against a timepoint reference
> > > stored when the first CAN message is received.
> > >
> > > When CAN bus traffic does not contain long idle pauses (so that
> > > the clocks would drift by a multiple of the counter rollover time),
> > > then the hardware timestamps provide precise relative time between
> > > received messages. This can be used e.g. for latency testing.
> >
> > Please make use of the existing cyclecounter/timecounter framework. Is
> > there a way to read the current time from a register? If so, please
> > setup a worker that does that regularly.
> >
> > Have a look at the mcp251xfd driver as an example:
> 
> Matej Vasilevski has looked at the example. But there is problem
> that we know no method how to read actual counter value at least for
> Xilinx Zynq 7000. May be we overlooked something or there
> is hidden test register.

I haven't found a documented register in the TRM. I've added Michal
Simek into the loop, maybe Michal has some connection to the HW
designers.

Marc

-- 
Pengutronix e.K.                 | Marc Kleine-Budde           |
Embedded Linux                   | https://www.pengutronix.de  |
Vertretung West/Dortmund         | Phone: +49-231-2826-924     |
Amtsgericht Hildesheim, HRA 2686 | Fax:   +49-5121-206917-5555 |

[-- Attachment #2: signature.asc --]
[-- Type: application/pgp-signature, Size: 488 bytes --]