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@xxxxxxxxx>
Signed-off-by: Matej Vasilevski <matej.vasilevski@xxxxxxxxx>
---
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
