struct clk *bus_clk;
struct clk *can_clk;
struct xcan_timepoint ref_timepoint;
- u16 prevhwts;
};
/* CAN Bittiming constants as per Xilinx CAN specs */
else
nrollovers = floor((now_ktime - ref_ktime)/rollover_ktime)
frame_ktime = ref_ktime + nrollovers * rollover_ktime + to_ns(tsdelta)
+ // must do fuzzy compare - the synchronization is not exact and so the frame_time
+ may actually be before now_time and still not be in the next period, just because
+ the latency between actual_frame_time and now_time may be lower than when
+ it was for the first frame, about which it was assumed the two times happened in the same moment
+ So compare it against half the period to be safe.
if frame_ktime > now_ktime
frame_ktime -= rollover_ktime
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, struct net_device *netdev, u32 *tmdiffns)
{
- u32 bitrate = priv->can.clock.freq;//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;
- int corr = 0;
-
now_ktime = ktime_get();
if (ktime_equal(ref->ktime, ns_to_ktime(0))) {
- //netdev_dbg(netdev, "TS: settings reference %hu = %llu; bitrate = %u", ts, ktime_to_ns(now_ktime), bitrate);
ref->ktime = now_ktime;
ref->ts = ts;
frame_ktime = now_ktime;
*tmdiffns = 0;
- //netdev_warn(netdev, "TS: sync");
} else {
+ s32 diff_us;
u16 tsdelta = (ts - ref->ts) & 0xFFFF;
-#define TS2NS(ts) ({ u64 tmp = (ts) * (u64)1000000000; do_div(tmp, bitrate); tmp; })
- // nrollovers = nowdelta / rollover_ns
- // nrollovers = nowdelta / (65536 * 1000000000 / bitrate)
- // nrollovers = nowdelta * bitrate / (65536 * 1000000000)
+#define TS2NS(ts) div_u64((ts) * (u64)1000000000, freq)
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) * bitrate), 65536 * 1000000000ULL);
-
- //netdev_dbg(netdev, "TS: rollover_ns = %lld, tsdelta = %hu, nowdelta = %llu, nrollovers = %u",
- // rollover_ns, tsdelta, ktime_to_ns(nowdelta), nrollovers);
-
- // nrollovers * rollover_ns + TS2NS(tsdelta)
- // nrollovers * (65536 * 1000000000 / bitrate) + (tsdelta * 1000000000 / bitrate)
- // (nrollovers * 65536 + tsdelta) * 1000000000 / bitrate;
- //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, bitrate));
- if (ktime_after(frame_ktime, now_ktime)) {
+ s64 nrollovers = ktime_divns(nowdelta, rollover_ns);
+
+ frame_ktime = ktime_add_ns(ref->ktime, nrollovers * rollover_ns + TS2NS(tsdelta));
+
+ diff_us = ktime_to_us(ktime_sub(now_ktime, frame_ktime));
+ diff_us = diff_us < 0 ? -diff_us : diff_us;
+
+ /* If the (magnitude of) delay is greater than half */
+ /* the period, the frame arrived in previous period */
+ if (diff_us*1000*2 > rollover_ns) {
frame_ktime = ktime_sub_ns(frame_ktime, rollover_ns);
- corr = 1;
}
- /*netdev_warn(netdev, "TS: ftime = %llu, delay = %lld us%s; hwtime = %hu, hwdiff = %hu, nrollovers = %u, "
- "tsdelta = %hu, bitrate = %u, ref->ktime = %llu, ref->ts = %hu, now_ktime = %llu",
+ /*
+ netdev_warn(netdev, "TS: ftime = %llu, delay = %lld us; hwtime = %hu, nrollovers = %llu, "
+ "now_ktime = %llu",
ktime_to_ns(frame_ktime),
ktime_to_us(ktime_sub(now_ktime, frame_ktime)),
- corr ? ", corr" : "",
ts,
- ts - priv->prevhwts,
- nrollovers, tsdelta, bitrate, ref->ktime, ref->ts, now_ktime);*/
-
- priv->prevhwts = ts;
+ nrollovers, ktime_to_ns(now_ktime));
+ */
*tmdiffns = ktime_to_ns(ktime_sub(now_ktime, frame_ktime)) & 0xFFFFFFFF;
#undef TS2NS
}
static u16 xcan_get_timestamp(u32 dlc)
{
- // TODO: endianity conversion?
return dlc & 0xFFFF;
}
struct sk_buff *skb;
struct skb_shared_hwtstamps *hwts;
u32 id_xcan, dlc_reg, dlc, rawts, data[2] = {0, 0};
- ktime_t ts, now_ktime;
u32 tmdiffns;
- //netdev_warn(ndev, "DBG: xcan_rx, %llu", ktime_to_ns(ktime_get()));
-
skb = alloc_can_skb(ndev, &cf);
if (unlikely(!skb)) {
stats->rx_dropped++;
hwts = skb_hwtstamps(skb);
memset(hwts, 0, sizeof(*hwts));
rawts = le32_to_cpu(xcan_get_timestamp(dlc_reg));
- ts = xcan_timestamp2ktime(priv, rawts, ndev, &tmdiffns);
- hwts->hwtstamp = ts;
- //netdev_dbg(ndev, "Frame timestamp: dlc 0x%08x, raw 0x%04hx = %hu, transformed %lld ns",
- // dlc_reg, rawts, rawts, ktime_to_ns(ts));
+ 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[0] = (data[0] & 0xFFFF0000) | (u16)((s16)(rawts - priv->ref_timepoint.ts)); // DBG
data[1] = tmdiffns; // DBG
-
+ cf->can_dlc = 8; // DBG
+ */
if (!(cf->can_id & CAN_RTR_FLAG)) {
/* Change Xilinx CAN data format to socketCAN data format */
if (cf->can_dlc > 0)
ier &= ~(XCAN_IXR_RXNEMP_MASK | XCAN_IXR_RXOK_MASK);
priv->write_reg(priv, XCAN_IER_OFFSET, ier);
napi_schedule(&priv->napi);
- //xcan_rx_poll(&priv->napi, 64);
}
return IRQ_HANDLED;
}
projects / fpga / zynq / canbench-sw.git / blobdiff