On 01.08.2022 20:46:54, Matej Vasilevski wrote: > This patch adds support for retrieving hardware timestamps to RX and > error CAN frames. It uses timecounter and cyclecounter structures, > because the timestamping counter width depends on the IP core integration > (it might not always be 64-bit). > For platform devices, you should specify "ts_clk" clock in device tree. > For PCI devices, the timestamping frequency is assumed to be the same > as bus frequency. > > Signed-off-by: Matej Vasilevski <matej.vasilevski@xxxxxxxxx> > --- > drivers/net/can/ctucanfd/Makefile | 2 +- > drivers/net/can/ctucanfd/ctucanfd.h | 20 ++ > drivers/net/can/ctucanfd/ctucanfd_base.c | 214 +++++++++++++++++- > drivers/net/can/ctucanfd/ctucanfd_timestamp.c | 87 +++++++ > 4 files changed, 315 insertions(+), 8 deletions(-) > create mode 100644 drivers/net/can/ctucanfd/ctucanfd_timestamp.c > > diff --git a/drivers/net/can/ctucanfd/Makefile b/drivers/net/can/ctucanfd/Makefile > index 8078f1f2c30f..a36e66f2cea7 100644 > --- a/drivers/net/can/ctucanfd/Makefile > +++ b/drivers/net/can/ctucanfd/Makefile > @@ -4,7 +4,7 @@ > # > > obj-$(CONFIG_CAN_CTUCANFD) := ctucanfd.o > -ctucanfd-y := ctucanfd_base.o > +ctucanfd-y := ctucanfd_base.o ctucanfd_timestamp.o > > obj-$(CONFIG_CAN_CTUCANFD_PCI) += ctucanfd_pci.o > obj-$(CONFIG_CAN_CTUCANFD_PLATFORM) += ctucanfd_platform.o > diff --git a/drivers/net/can/ctucanfd/ctucanfd.h b/drivers/net/can/ctucanfd/ctucanfd.h > index 0e9904f6a05d..43d9c73ce244 100644 > --- a/drivers/net/can/ctucanfd/ctucanfd.h > +++ b/drivers/net/can/ctucanfd/ctucanfd.h > @@ -23,6 +23,10 @@ > #include <linux/netdevice.h> > #include <linux/can/dev.h> > #include <linux/list.h> > +#include <linux/timecounter.h> > +#include <linux/workqueue.h> > + > +#define CTUCANFD_MAX_WORK_DELAY_SEC 86400U /* one day == 24 * 3600 seconds */ For higher precision we can limit this to 3600s, as the sched_clock does it : | https://elixir.bootlin.com/linux/v5.19/source/kernel/time/sched_clock.c#L169 > enum ctu_can_fd_can_registers; > > @@ -51,6 +55,15 @@ struct ctucan_priv { > u32 rxfrm_first_word; > > struct list_head peers_on_pdev; > + > + struct cyclecounter cc; > + struct timecounter tc; > + struct delayed_work timestamp; > + > + struct clk *timestamp_clk; > + u32 work_delay_jiffies; schedule_delayed_work() takes an "unsigned long" not a u32. > + bool timestamp_enabled; > + bool timestamp_possible; > }; > > /** > @@ -79,4 +92,11 @@ int ctucan_probe_common(struct device *dev, void __iomem *addr, > int ctucan_suspend(struct device *dev) __maybe_unused; > int ctucan_resume(struct device *dev) __maybe_unused; > > +u64 ctucan_read_timestamp_cc_wrapper(const struct cyclecounter *cc); > +u64 ctucan_read_timestamp_counter(struct ctucan_priv *priv); > +u32 ctucan_calculate_work_delay(const u32 timestamp_bit_size, const u32 timestamp_freq); > +void ctucan_skb_set_timestamp(const struct ctucan_priv *priv, struct sk_buff *skb, > + u64 timestamp); > +void ctucan_timestamp_init(struct ctucan_priv *priv); > +void ctucan_timestamp_stop(struct ctucan_priv *priv); > #endif /*__CTUCANFD__*/ > diff --git a/drivers/net/can/ctucanfd/ctucanfd_base.c b/drivers/net/can/ctucanfd/ctucanfd_base.c > index 3c18d028bd8c..35b37de51811 100644 > --- a/drivers/net/can/ctucanfd/ctucanfd_base.c > +++ b/drivers/net/can/ctucanfd/ctucanfd_base.c > @@ -18,6 +18,7 @@ > ******************************************************************************/ > > #include <linux/clk.h> > +#include <linux/clocksource.h> > #include <linux/errno.h> > #include <linux/ethtool.h> > #include <linux/init.h> > @@ -148,6 +149,27 @@ static void ctucan_write_txt_buf(struct ctucan_priv *priv, enum ctu_can_fd_can_r > priv->write_reg(priv, buf_base + offset, val); > } > > +static u64 concatenate_two_u32(u32 high, u32 low) static inline? > +{ > + return ((u64)high << 32) | ((u64)low); > +} > + > +u64 ctucan_read_timestamp_counter(struct ctucan_priv *priv) > +{ > + u32 ts_low; > + u32 ts_high; > + u32 ts_high2; > + > + ts_high = ctucan_read32(priv, CTUCANFD_TIMESTAMP_HIGH); > + ts_low = ctucan_read32(priv, CTUCANFD_TIMESTAMP_LOW); > + ts_high2 = ctucan_read32(priv, CTUCANFD_TIMESTAMP_HIGH); > + > + if (ts_high2 != ts_high) > + ts_low = priv->read_reg(priv, CTUCANFD_TIMESTAMP_LOW); > + > + return concatenate_two_u32(ts_high2, ts_low) & priv->cc.mask; > +} > + > #define CTU_CAN_FD_TXTNF(priv) (!!FIELD_GET(REG_STATUS_TXNF, ctucan_read32(priv, CTUCANFD_STATUS))) > #define CTU_CAN_FD_ENABLED(priv) (!!FIELD_GET(REG_MODE_ENA, ctucan_read32(priv, CTUCANFD_MODE))) please make these static inline bool functions. > > @@ -640,12 +662,16 @@ static netdev_tx_t ctucan_start_xmit(struct sk_buff *skb, struct net_device *nde > * @priv: Pointer to CTU CAN FD's private data > * @cf: Pointer to CAN frame struct > * @ffw: Previously read frame format word > + * @skb: Pointer to buffer to store timestamp > * > * Note: Frame format word must be read separately and provided in 'ffw'. > */ > -static void ctucan_read_rx_frame(struct ctucan_priv *priv, struct canfd_frame *cf, u32 ffw) > +static void ctucan_read_rx_frame(struct ctucan_priv *priv, struct canfd_frame *cf, > + u32 ffw, u64 *timestamp) > { > u32 idw; > + u32 tstamp_high; > + u32 tstamp_low; > unsigned int i; > unsigned int wc; > unsigned int len; > @@ -682,9 +708,10 @@ static void ctucan_read_rx_frame(struct ctucan_priv *priv, struct canfd_frame *c > if (unlikely(len > wc * 4)) > len = wc * 4; > > - /* Timestamp - Read and throw away */ > - ctucan_read32(priv, CTUCANFD_RX_DATA); > - ctucan_read32(priv, CTUCANFD_RX_DATA); > + /* Timestamp */ > + tstamp_low = ctucan_read32(priv, CTUCANFD_RX_DATA); > + tstamp_high = ctucan_read32(priv, CTUCANFD_RX_DATA); > + *timestamp = concatenate_two_u32(tstamp_high, tstamp_low) & priv->cc.mask; > > /* Data */ > for (i = 0; i < len; i += 4) { > @@ -713,6 +740,7 @@ static int ctucan_rx(struct net_device *ndev) > struct net_device_stats *stats = &ndev->stats; > struct canfd_frame *cf; > struct sk_buff *skb; > + u64 timestamp; > u32 ffw; > > if (test_bit(CTUCANFD_FLAG_RX_FFW_BUFFERED, &priv->drv_flags)) { > @@ -736,7 +764,9 @@ static int ctucan_rx(struct net_device *ndev) > return 0; > } > > - ctucan_read_rx_frame(priv, cf, ffw); > + ctucan_read_rx_frame(priv, cf, ffw, ×tamp); > + if (priv->timestamp_enabled) > + ctucan_skb_set_timestamp(priv, skb, timestamp); Can the ctucan_skb_set_timestamp() and ctucan_read_timestamp_counter() happen concurrently? AFAICS they are all called from ctucan_rx_poll(), right? > > stats->rx_bytes += cf->len; > stats->rx_packets++; > @@ -906,6 +936,11 @@ static void ctucan_err_interrupt(struct net_device *ndev, u32 isr) > if (skb) { > stats->rx_packets++; > stats->rx_bytes += cf->can_dlc; > + if (priv->timestamp_enabled) { > + u64 tstamp = ctucan_read_timestamp_counter(priv); > + > + ctucan_skb_set_timestamp(priv, skb, tstamp); > + } > netif_rx(skb); > } > } > @@ -951,6 +986,11 @@ static int ctucan_rx_poll(struct napi_struct *napi, int quota) > cf->data[1] |= CAN_ERR_CRTL_RX_OVERFLOW; > stats->rx_packets++; > stats->rx_bytes += cf->can_dlc; > + if (priv->timestamp_enabled) { > + u64 tstamp = ctucan_read_timestamp_counter(priv); > + > + ctucan_skb_set_timestamp(priv, skb, tstamp); > + } > netif_rx(skb); > } > > @@ -1231,6 +1271,9 @@ static int ctucan_open(struct net_device *ndev) > goto err_chip_start; > } > > + if (priv->timestamp_possible) > + ctucan_timestamp_init(priv); > + > netdev_info(ndev, "ctu_can_fd device registered\n"); > napi_enable(&priv->napi); > netif_start_queue(ndev); > @@ -1263,6 +1306,9 @@ static int ctucan_close(struct net_device *ndev) > ctucan_chip_stop(ndev); > free_irq(ndev->irq, ndev); > close_candev(ndev); > + if (priv->timestamp_possible) > + ctucan_timestamp_stop(priv); > + Nitpick: Don't add an extra newline here. > > pm_runtime_put(priv->dev); > > @@ -1295,15 +1341,117 @@ static int ctucan_get_berr_counter(const struct net_device *ndev, struct can_ber > return 0; > } > > +static int ctucan_hwtstamp_set(struct net_device *dev, struct ifreq *ifr) > +{ > + struct ctucan_priv *priv = netdev_priv(dev); > + struct hwtstamp_config cfg; > + > + if (!priv->timestamp_possible) > + 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->timestamp_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->timestamp_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 ctucan_hwtstamp_get(struct net_device *dev, struct ifreq *ifr) > +{ > + struct ctucan_priv *priv = netdev_priv(dev); > + struct hwtstamp_config cfg; > + > + if (!priv->timestamp_possible) > + return -EOPNOTSUPP; > + > + cfg.flags = 0; > + cfg.tx_type = HWTSTAMP_TX_OFF; > + cfg.rx_filter = priv->timestamp_enabled ? HWTSTAMP_FILTER_ALL : HWTSTAMP_FILTER_NONE; > + > + return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; > +} > + > +static int ctucan_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd) > +{ > + switch (cmd) { > + case SIOCSHWTSTAMP: > + return ctucan_hwtstamp_set(dev, ifr); > + case SIOCGHWTSTAMP: > + return ctucan_hwtstamp_get(dev, ifr); > + default: > + return -EOPNOTSUPP; > + } > +} > + > +static int ctucan_ethtool_get_ts_info(struct net_device *ndev, struct ethtool_ts_info *info) > +{ > + struct ctucan_priv *priv = netdev_priv(ndev); > + > + ethtool_op_get_ts_info(ndev, info); > + > + if (!priv->timestamp_possible) > + return 0; > + > + info->so_timestamping |= SOF_TIMESTAMPING_RX_HARDWARE | > + SOF_TIMESTAMPING_RAW_HARDWARE; > + info->tx_types = BIT(HWTSTAMP_TX_OFF); > + info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | > + BIT(HWTSTAMP_FILTER_ALL); > + > + return 0; > +} > + > static const struct net_device_ops ctucan_netdev_ops = { > .ndo_open = ctucan_open, > .ndo_stop = ctucan_close, > .ndo_start_xmit = ctucan_start_xmit, > .ndo_change_mtu = can_change_mtu, > + .ndo_eth_ioctl = ctucan_ioctl, > }; > > static const struct ethtool_ops ctucan_ethtool_ops = { > - .get_ts_info = ethtool_op_get_ts_info, > + .get_ts_info = ctucan_ethtool_get_ts_info, > }; > > int ctucan_suspend(struct device *dev) > @@ -1345,6 +1493,8 @@ int ctucan_probe_common(struct device *dev, void __iomem *addr, int irq, unsigne > struct ctucan_priv *priv; > struct net_device *ndev; > int ret; > + u32 timestamp_freq = 0; > + u32 timestamp_bit_size = 0; Nitpick: please move the u32 between the struct and the int. > > /* Create a CAN device instance */ > ndev = alloc_candev(sizeof(struct ctucan_priv), ntxbufs); > @@ -1386,7 +1536,9 @@ int ctucan_probe_common(struct device *dev, void __iomem *addr, int irq, unsigne > > /* Getting the can_clk info */ > if (!can_clk_rate) { > - priv->can_clk = devm_clk_get(dev, NULL); > + priv->can_clk = devm_clk_get_optional(dev, "core-clk"); > + if (!priv->can_clk) > + priv->can_clk = devm_clk_get(dev, NULL); Please add a comment here, that the NULL clock is for compatibility with older DTs. > if (IS_ERR(priv->can_clk)) { > dev_err(dev, "Device clock not found.\n"); > ret = PTR_ERR(priv->can_clk); > @@ -1425,6 +1577,54 @@ int ctucan_probe_common(struct device *dev, void __iomem *addr, int irq, unsigne > > priv->can.clock.freq = can_clk_rate; > > + priv->timestamp_enabled = false; > + priv->timestamp_possible = true; > + priv->timestamp_clk = NULL; priv is allocated and initialized with 0 > + > + /* Obtain timestamping frequency */ > + if (pm_enable_call) { > + /* Plaftorm device: get tstamp clock from device tree */ > + priv->timestamp_clk = devm_clk_get(dev, "ts-clk"); > + if (IS_ERR(priv->timestamp_clk)) { > + /* Take the core clock frequency instead */ > + timestamp_freq = can_clk_rate; > + } else { > + timestamp_freq = clk_get_rate(priv->timestamp_clk); > + } Who prepares/enabled the timestamp clock? clk_get_rate() is only valid if the clock is enabled. I know, we violate this for the CAN clock. :/ > + } else { > + /* PCI device: assume tstamp freq is equal to bus clk rate */ > + timestamp_freq = can_clk_rate; > + } > + > + /* Obtain timestamping counter bit size */ > + timestamp_bit_size = (ctucan_read32(priv, CTUCANFD_ERR_CAPT) & REG_ERR_CAPT_TS_BITS) >> 24; > + timestamp_bit_size += 1; /* the register value was bit_size - 1 */ Please move the +1 into the else of the following if() which results in: | if (timestamp_bit_size) which is IMHO easier to read. > + > + /* For 2.x versions of the IP core, we will assume 64-bit counter > + * if there was a 0 in the register. > + */ > + if (timestamp_bit_size == 1) { > + u32 version_reg = ctucan_read32(priv, CTUCANFD_DEVICE_ID); > + u32 major = (version_reg & REG_DEVICE_ID_VER_MAJOR) >> 24; > + > + if (major == 2) > + timestamp_bit_size = 64; > + else > + priv->timestamp_possible = false; > + } > + > + /* Setup conversion constants and work delay */ > + priv->cc.read = ctucan_read_timestamp_cc_wrapper; > + priv->cc.mask = CYCLECOUNTER_MASK(timestamp_bit_size); Does the driver use these 2 if timestamping is not possible? > + if (priv->timestamp_possible) { > + clocks_calc_mult_shift(&priv->cc.mult, &priv->cc.shift, timestamp_freq, > + NSEC_PER_SEC, CTUCANFD_MAX_WORK_DELAY_SEC); > + priv->work_delay_jiffies = > + ctucan_calculate_work_delay(timestamp_bit_size, timestamp_freq); > + if (priv->work_delay_jiffies == 0) > + priv->timestamp_possible = false; You'll get a higher precision if you take the mask into account, at least if the counter overflows before CTUCANFD_MAX_WORK_DELAY_SEC: maxsec = min(CTUCANFD_MAX_WORK_DELAY_SEC, priv->cc.mask / timestamp_freq); clocks_calc_mult_shift(&priv->cc.mult, &priv->cc.shift, timestamp_freq, NSEC_PER_SEC, maxsec); work_delay_in_ns = clocks_calc_max_nsecs(&priv->cc.mult, &priv->cc.shift, 0, &priv->cc.mask, NULL); You can use clocks_calc_max_nsecs() to calculate the work delay. regards, 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:
signature.asc
Description: PGP signature
