Add support for hardware based timestamping.
Signed-off-by: Marc Kleine-Budde <mkl@xxxxxxxxxxxxxx>
---
drivers/net/can/rockchip/rockchip_canfd-core.c | 6 ++
drivers/net/can/rockchip/rockchip_canfd-ethtool.c | 2 +-
drivers/net/can/rockchip/rockchip_canfd-rx.c | 1 +
.../net/can/rockchip/rockchip_canfd-timestamp.c | 94 +++++++++++++++++++++-
drivers/net/can/rockchip/rockchip_canfd-tx.c | 2 +
drivers/net/can/rockchip/rockchip_canfd.h | 10 +++
6 files changed, 112 insertions(+), 3 deletions(-)
diff --git a/drivers/net/can/rockchip/rockchip_canfd-core.c b/drivers/net/can/rockchip/rockchip_canfd-core.c
index 9c762b59e310..acbbfdd4cb69 100644
--- 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 0084f37b2b9f..5aeeef64a67a 100644
--- 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 fa7913ef415d..411b0cf5487a 100644
--- 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 9301b3ceceb0..81cccc5fd838 100644
--- 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@xxxxxxxxxxxxxx>
//
+#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 d719a52258e3..f954f38b955f 100644
--- a/drivers/net/can/rockchip/rockchip_canfd-tx.c
+++ b/drivers/net/can/rockchip/rockchip_canfd-tx.c
@@ -157,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 f06f6a83b77f..aa3064101721 100644
--- a/drivers/net/can/rockchip/rockchip_canfd.h
+++ b/drivers/net/can/rockchip/rockchip_canfd.h
@@ -468,6 +468,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;
@@ -530,7 +535,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);
--
2.43.0
