can: rockchip_canfd: add hardware timestamping support

Add support for hardware based timestamping.

Tested-by: Alibek Omarov <a1ba.omarov@gmail.com>
Acked-by: Heiko Stuebner <heiko@sntech.de>
Link: https://patch.msgid.link/20240904-rockchip-canfd-v5-18-8ae22bcb27cc@pengutronix.de
Signed-off-by: Marc Kleine-Budde <mkl@pengutronix.de>

diff --git a/drivers/net/can/rockchip/rockchip_canfd-core.c b/drivers/net/can/rockchip/rockchip_canfd-core.c
index 4db552bfc4db4df1c5e0a6029e51b4e889596dfb..015623314e8886b6216af73610c937e58074d252 100644 (file)
--- a/drivers/net/can/rockchip/rockchip_canfd-core.c
+++ b/drivers/net/can/rockchip/rockchip_canfd-core.c
@@ -292,6 +292,8 @@ static void rkcanfd_chip_start(struct rkcanfd_priv *priv)
 
        rkcanfd_chip_fifo_setup(priv);
        rkcanfd_timestamp_init(priv);
+       rkcanfd_timestamp_start(priv);
+
        rkcanfd_set_bittiming(priv);
 
        rkcanfd_chip_interrupts_disable(priv);
@@ -315,6 +317,7 @@ static void rkcanfd_chip_stop(struct rkcanfd_priv *priv, const enum can_state st
 {
        priv->can.state = state;
 
+       rkcanfd_timestamp_stop(priv);
        __rkcanfd_chip_stop(priv, state);
 }
 
@@ -322,6 +325,7 @@ static void rkcanfd_chip_stop_sync(struct rkcanfd_priv *priv, const enum can_sta
 {
        priv->can.state = state;
 
+       rkcanfd_timestamp_stop_sync(priv);
        __rkcanfd_chip_stop(priv, state);
 }
 
@@ -353,6 +357,8 @@ rkcanfd_alloc_can_err_skb(struct rkcanfd_priv *priv,
        *timestamp = rkcanfd_get_timestamp(priv);
 
        skb = alloc_can_err_skb(priv->ndev, cf);
+       if (skb)
+               rkcanfd_skb_set_timestamp(priv, skb, *timestamp);
 
        return skb;
 }

diff --git a/drivers/net/can/rockchip/rockchip_canfd-ethtool.c b/drivers/net/can/rockchip/rockchip_canfd-ethtool.c
index 0084f37b2b9fcf522b785cd2b28d6e2d3c021008..5aeeef64a67a058691a85358a67ee4a6a4254519 100644 (file)
--- a/drivers/net/can/rockchip/rockchip_canfd-ethtool.c
+++ b/drivers/net/can/rockchip/rockchip_canfd-ethtool.c
@@ -59,7 +59,7 @@ rkcanfd_ethtool_get_ethtool_stats(struct net_device *ndev,
 }
 
 static const struct ethtool_ops rkcanfd_ethtool_ops = {
-       .get_ts_info = ethtool_op_get_ts_info,
+       .get_ts_info = can_ethtool_op_get_ts_info_hwts,
        .get_strings = rkcanfd_ethtool_get_strings,
        .get_sset_count = rkcanfd_ethtool_get_sset_count,
        .get_ethtool_stats = rkcanfd_ethtool_get_ethtool_stats,

diff --git a/drivers/net/can/rockchip/rockchip_canfd-rx.c b/drivers/net/can/rockchip/rockchip_canfd-rx.c
index bacef5e5dc395b4ead9ff5c7fe5921b6d60b634d..d862116840eba7463df091174b56aba5b2844e5f 100644 (file)
--- a/drivers/net/can/rockchip/rockchip_canfd-rx.c
+++ b/drivers/net/can/rockchip/rockchip_canfd-rx.c
@@ -267,6 +267,7 @@ static int rkcanfd_handle_rx_int_one(struct rkcanfd_priv *priv)
        }
 
        memcpy(skb_cfd, cfd, len);
+       rkcanfd_skb_set_timestamp(priv, skb, header->ts);
 
        err = can_rx_offload_queue_timestamp(&priv->offload, skb, header->ts);
        if (err)

diff --git a/drivers/net/can/rockchip/rockchip_canfd-timestamp.c b/drivers/net/can/rockchip/rockchip_canfd-timestamp.c
index 9301b3ceceb08b1619896d2441faaebe7d4375ae..81cccc5fd8384ee1fec919077db48632f0fe7cc2 100644 (file)
--- a/drivers/net/can/rockchip/rockchip_canfd-timestamp.c
+++ b/drivers/net/can/rockchip/rockchip_canfd-timestamp.c
@@ -4,12 +4,102 @@
 //               Marc Kleine-Budde <kernel@pengutronix.de>
 //
 
+#include <linux/clocksource.h>
+
 #include "rockchip_canfd.h"
 
+static u64 rkcanfd_timestamp_read(const struct cyclecounter *cc)
+{
+       const struct rkcanfd_priv *priv = container_of(cc, struct rkcanfd_priv, cc);
+
+       return rkcanfd_get_timestamp(priv);
+}
+
+void rkcanfd_skb_set_timestamp(const struct rkcanfd_priv *priv,
+                              struct sk_buff *skb, const u32 timestamp)
+{
+       struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);
+       u64 ns;
+
+       ns = timecounter_cyc2time(&priv->tc, timestamp);
+
+       hwtstamps->hwtstamp = ns_to_ktime(ns);
+}
+
+static void rkcanfd_timestamp_work(struct work_struct *work)
+{
+       const struct delayed_work *delayed_work = to_delayed_work(work);
+       struct rkcanfd_priv *priv;
+
+       priv = container_of(delayed_work, struct rkcanfd_priv, timestamp);
+       timecounter_read(&priv->tc);
+
+       schedule_delayed_work(&priv->timestamp, priv->work_delay_jiffies);
+}
+
 void rkcanfd_timestamp_init(struct rkcanfd_priv *priv)
 {
-       u32 reg;
+       const struct can_bittiming *dbt = &priv->can.data_bittiming;
+       const struct can_bittiming *bt = &priv->can.bittiming;
+       struct cyclecounter *cc = &priv->cc;
+       u32 bitrate, div, reg, rate;
+       u64 work_delay_ns;
+       u64 max_cycles;
 
-       reg = RKCANFD_REG_TIMESTAMP_CTRL_TIME_BASE_COUNTER_ENABLE;
+       /* At the standard clock rate of 300Mhz on the rk3658, the 32
+        * bit timer overflows every 14s. This means that we have to
+        * poll it quite often to avoid missing a wrap around.
+        *
+        * Divide it down to a reasonable rate, at least twice the bit
+        * rate.
+        */
+       bitrate = max(bt->bitrate, dbt->bitrate);
+       div = min(DIV_ROUND_UP(priv->can.clock.freq, bitrate * 2),
+                 FIELD_MAX(RKCANFD_REG_TIMESTAMP_CTRL_TIME_BASE_COUNTER_PRESCALE) + 1);
+
+       reg = FIELD_PREP(RKCANFD_REG_TIMESTAMP_CTRL_TIME_BASE_COUNTER_PRESCALE,
+                        div - 1) |
+               RKCANFD_REG_TIMESTAMP_CTRL_TIME_BASE_COUNTER_ENABLE;
        rkcanfd_write(priv, RKCANFD_REG_TIMESTAMP_CTRL, reg);
+
+       cc->read = rkcanfd_timestamp_read;
+       cc->mask = CYCLECOUNTER_MASK(32);
+
+       rate = priv->can.clock.freq / div;
+       clocks_calc_mult_shift(&cc->mult, &cc->shift, rate, NSEC_PER_SEC,
+                              RKCANFD_TIMESTAMP_WORK_MAX_DELAY_SEC);
+
+       max_cycles = div_u64(ULLONG_MAX, cc->mult);
+       max_cycles = min(max_cycles, cc->mask);
+       work_delay_ns = clocksource_cyc2ns(max_cycles, cc->mult, cc->shift) / 3;
+       priv->work_delay_jiffies = nsecs_to_jiffies(work_delay_ns);
+       INIT_DELAYED_WORK(&priv->timestamp, rkcanfd_timestamp_work);
+
+       netdev_dbg(priv->ndev, "clock=%lu.%02luMHz bitrate=%lu.%02luMBit/s div=%u rate=%lu.%02luMHz mult=%u shift=%u delay=%lus\n",
+                  priv->can.clock.freq / MEGA,
+                  priv->can.clock.freq % MEGA / KILO / 10,
+                  bitrate / MEGA,
+                  bitrate % MEGA / KILO / 100,
+                  div,
+                  rate / MEGA,
+                  rate % MEGA / KILO / 10,
+                  cc->mult, cc->shift,
+                  priv->work_delay_jiffies / HZ);
+}
+
+void rkcanfd_timestamp_start(struct rkcanfd_priv *priv)
+{
+       timecounter_init(&priv->tc, &priv->cc, ktime_get_real_ns());
+
+       schedule_delayed_work(&priv->timestamp, priv->work_delay_jiffies);
+}
+
+void rkcanfd_timestamp_stop(struct rkcanfd_priv *priv)
+{
+       cancel_delayed_work(&priv->timestamp);
+}
+
+void rkcanfd_timestamp_stop_sync(struct rkcanfd_priv *priv)
+{
+       cancel_delayed_work_sync(&priv->timestamp);
 }

diff --git a/drivers/net/can/rockchip/rockchip_canfd-tx.c b/drivers/net/can/rockchip/rockchip_canfd-tx.c
index d10da548ba714e2d3e222202ded31dedaafb7d97..f954f38b955f6dc26b856e4c2f40e20256a7f114 100644 (file)
--- a/drivers/net/can/rockchip/rockchip_canfd-tx.c
+++ b/drivers/net/can/rockchip/rockchip_canfd-tx.c
@@ -145,8 +145,10 @@ void rkcanfd_handle_tx_done_one(struct rkcanfd_priv *priv, const u32 ts,
 {
        struct net_device_stats *stats = &priv->ndev->stats;
        unsigned int tx_tail;
+       struct sk_buff *skb;
 
        tx_tail = rkcanfd_get_tx_tail(priv);
+       skb = priv->can.echo_skb[tx_tail];
 
        /* Manual handling of CAN Bus Error counters. See
         * rkcanfd_get_corrected_berr_counter() for detailed
@@ -155,6 +157,8 @@ void rkcanfd_handle_tx_done_one(struct rkcanfd_priv *priv, const u32 ts,
        if (priv->bec.txerr)
                priv->bec.txerr--;
 
+       if (skb)
+               rkcanfd_skb_set_timestamp(priv, skb, ts);
        stats->tx_bytes +=
                can_rx_offload_get_echo_skb_queue_timestamp(&priv->offload,
                                                            tx_tail, ts,

diff --git a/drivers/net/can/rockchip/rockchip_canfd.h b/drivers/net/can/rockchip/rockchip_canfd.h
index 6be2865ec95a61c9a1a22f929ad578db8996c7a0..3efd7f174e14c2b67b1feef897bc34336f578226 100644 (file)
--- a/drivers/net/can/rockchip/rockchip_canfd.h
+++ b/drivers/net/can/rockchip/rockchip_canfd.h
@@ -15,6 +15,7 @@
 #include <linux/netdevice.h>
 #include <linux/reset.h>
 #include <linux/skbuff.h>
+#include <linux/timecounter.h>
 #include <linux/types.h>
 #include <linux/u64_stats_sync.h>
 #include <linux/units.h>
@@ -469,6 +470,11 @@ struct rkcanfd_priv {
        u32 reg_int_mask_default;
        struct rkcanfd_devtype_data devtype_data;
 
+       struct cyclecounter cc;
+       struct timecounter tc;
+       struct delayed_work timestamp;
+       unsigned long work_delay_jiffies;
+
        struct can_berr_counter bec;
 
        struct rkcanfd_stats stats;
@@ -531,7 +537,12 @@ void rkcanfd_ethtool_init(struct rkcanfd_priv *priv);
 
 int rkcanfd_handle_rx_int(struct rkcanfd_priv *priv);
 
+void rkcanfd_skb_set_timestamp(const struct rkcanfd_priv *priv,
+                              struct sk_buff *skb, const u32 timestamp);
 void rkcanfd_timestamp_init(struct rkcanfd_priv *priv);
+void rkcanfd_timestamp_start(struct rkcanfd_priv *priv);
+void rkcanfd_timestamp_stop(struct rkcanfd_priv *priv);
+void rkcanfd_timestamp_stop_sync(struct rkcanfd_priv *priv);
 
 unsigned int rkcanfd_get_effective_tx_free(const struct rkcanfd_priv *priv);
 void rkcanfd_xmit_retry(struct rkcanfd_priv *priv);