set reference point (ref_ktime, ref_ts) = (frame_ktime, frame_ts) ??? cummulative loss of precision ? meybe not
*/
-static ktime_t xcan_timestamp2ktime(struct xcan_priv *priv, u16 ts)
+static ktime_t xcan_timestamp2ktime(struct xcan_priv *priv, u16 ts, struct net_device *netdev, u32 *tmdiffns)
{
- u32 bitrate = priv->can.bittiming.bitrate; // TODO: I once saw a function for this ...
+ u32 freq = priv->can.clock.freq;
struct xcan_timepoint *ref = &priv->ref_timepoint;
- ktime_t now_ktime = ktime_get();
ktime_t frame_ktime;
+ ktime_t now_ktime;
+
+ now_ktime = ktime_get();
if (ktime_equal(ref->ktime, ns_to_ktime(0))) {
ref->ktime = now_ktime;
ref->ts = ts;
frame_ktime = now_ktime;
+ *tmdiffns = 0;
} else {
u16 tsdelta = (ts - ref->ts) & 0xFFFF;
-#define TS2NS(ts) ({ u64 tmp = (ts) * (u64)1000000000; do_div(tmp, bitrate); tmp; })
- s64 rollover_ns = TS2NS(ts);
+#define TS2NS(ts) div_u64((ts) * (u64)1000000000, freq)
+ // nrollovers = nowdelta / rollover_ns
+ // nrollovers = nowdelta / (65536 * 1000000000 / freq)
+ // nrollovers = nowdelta * freq / (65536 * 1000000000)
+ s64 rollover_ns = TS2NS(65536);
ktime_t nowdelta = ktime_sub(now_ktime, ref->ktime);
u32 nrollovers = ktime_divns(nowdelta, rollover_ns);
+ //u32 nrollovers = ktime_divns(ns_to_ktime(ktime_to_ns(nowdelta) * freq), 65536 * 1000000000ULL);
frame_ktime = ktime_add_ns(ref->ktime, nrollovers * rollover_ns + TS2NS(tsdelta));
+ //frame_ktime = ktime_add_ns(ref->ktime, div_u64(((u64)nrollovers * 65536 + tsdelta) * 1000000000, freq));
if (ktime_after(frame_ktime, now_ktime)) {
frame_ktime = ktime_sub_ns(frame_ktime, rollover_ns);
}
+ /*netdev_warn(netdev, "TS: ftime = %llu, delay = %lld us%s; hwtime = %hu, nrollovers = %u, "
+ "tsdelta = %hu, freq = %u, ref->ktime = %llu, ref->ts = %hu, now_ktime = %llu",
+ ktime_to_ns(frame_ktime),
+ ktime_to_us(ktime_sub(now_ktime, frame_ktime)),
+ corr ? ", corr" : "",
+ ts,
+ nrollovers, tsdelta, freq, ref->ktime, ref->ts, now_ktime);*/
+
+ *tmdiffns = ktime_to_ns(ktime_sub(now_ktime, frame_ktime)) & 0xFFFFFFFF;
#undef TS2NS
}
return frame_ktime;
static u16 xcan_get_timestamp(u32 dlc)
{
- // TODO: endianity conversion?
return dlc & 0xFFFF;
}
struct can_frame *cf;
struct sk_buff *skb;
struct skb_shared_hwtstamps *hwts;
- u32 id_xcan, dlc, data[2] = {0, 0};
- ktime_t ts;
+ u32 id_xcan, dlc_reg, dlc, rawts, data[2] = {0, 0};
+ u32 tmdiffns;
skb = alloc_can_skb(ndev, &cf);
if (unlikely(!skb)) {
/* Read a frame from Xilinx zynq CANPS */
id_xcan = priv->read_reg(priv, XCAN_RXFIFO_ID_OFFSET);
- dlc = priv->read_reg(priv, XCAN_RXFIFO_DLC_OFFSET) >>
- XCAN_DLCR_DLC_SHIFT;
+ dlc_reg = priv->read_reg(priv, XCAN_RXFIFO_DLC_OFFSET);
+ dlc = dlc_reg >> XCAN_DLCR_DLC_SHIFT;
/* Change Xilinx CAN data length format to socketCAN data format */
cf->can_dlc = get_can_dlc(dlc);
/* Capture HW timestamp. TODO: only if requested? */
hwts = skb_hwtstamps(skb);
memset(hwts, 0, sizeof(*hwts));
- ts = xcan_timestamp2ktime(priv, le32_to_cpu(xcan_get_timestamp(dlc)));
- hwts->hwtstamp = ts;
- netdev_dbg(ndev, "Frame timestamp: raw %hu, transformed %lld ns", xcan_get_timestamp(dlc), ktime_to_ns(ts));
+ rawts = le32_to_cpu(xcan_get_timestamp(dlc_reg));
+ hwts->hwtstamp = xcan_timestamp2ktime(priv, rawts, ndev, &tmdiffns);
/* Change Xilinx CAN ID format to socketCAN ID format */
if (id_xcan & XCAN_IDR_IDE_MASK) {
/* DW1/DW2 must always be read to remove message from RXFIFO */
data[0] = priv->read_reg(priv, XCAN_RXFIFO_DW1_OFFSET);
data[1] = priv->read_reg(priv, XCAN_RXFIFO_DW2_OFFSET);
+ //data[1] = tmdiffns; // DBG
if (!(cf->can_id & CAN_RTR_FLAG)) {
/* Change Xilinx CAN data format to socketCAN data format */
u32 isr, ier;
int work_done = 0;
+ //netdev_warn(ndev, "DBG: xcan_rx_poll");
isr = priv->read_reg(priv, XCAN_ISR_OFFSET);
while ((isr & XCAN_IXR_RXNEMP_MASK) && (work_done < quota)) {
- if (isr & XCAN_IXR_RXOK_MASK) {
+ //if (isr & XCAN_IXR_RXOK_MASK) {
priv->write_reg(priv, XCAN_ICR_OFFSET,
XCAN_IXR_RXOK_MASK);
work_done += xcan_rx(ndev);
- } else {
+ /*} else {
priv->write_reg(priv, XCAN_ICR_OFFSET,
XCAN_IXR_RXNEMP_MASK);
break;
- }
+ }*/
priv->write_reg(priv, XCAN_ICR_OFFSET, XCAN_IXR_RXNEMP_MASK);
isr = priv->read_reg(priv, XCAN_ISR_OFFSET);
}
/* Get the interrupt status from Xilinx CAN */
isr = priv->read_reg(priv, XCAN_ISR_OFFSET);
+ //netdev_warn(ndev, "DBG: xcan_interrupt: ISR = 0x%08x, IER = 0x%08x", isr, priv->read_reg(priv, XCAN_IER_OFFSET));
if (!isr)
return IRQ_NONE;