FPGA/corundum
1
0
Fork 0
mirror of https://github.com/corundum/corundum.git synced 2025-01-16 08:12:53 +08:00
Code Issues Projects Releases Wiki Activity
corundum/tb/ptp_td.py
Alex Forencich bd8e8e5b20 Add PTP time distribution components 
Signed-off-by: Alex Forencich <alex@alexforencich.com>
2023-11-07 13:07:15 -08:00

605 lines
22 KiB
Python
Raw Blame History

"""
Copyright (c) 2023 Alex Forencich
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
"""
import logging
from decimal import Decimal, Context
from fractions import Fraction
import cocotb
from cocotb.triggers import RisingEdge, Event
from cocotb.utils import get_sim_time
from cocotbext.eth.reset import Reset
class PtpTdSource(Reset):
def __init__(self,
data=None,
clock=None,
reset=None,
reset_active_level=True,
period_ns=6.4,
td_delay=32,
*args, **kwargs):
self.log = logging.getLogger(f"cocotb.{data._path}")
self.data = data
self.clock = clock
self.reset = reset
self.log.info("PTP time distribution source")
self.log.info("Copyright (c) 2023 Alex Forencich")
self.log.info("https://github.com/alexforencich/verilog-ethernet")
super().__init__(*args, **kwargs)
self.ctx = Context(prec=60)
self.period_ns = 0
self.period_fns = 0
self.drift_num = 0
self.drift_denom = 0
self.drift_cnt = 0
self.set_period_ns(period_ns)
self.ts_fns = 0
self.ts_rel_ns = 0
self.ts_rel_updated = False
self.ts_tod_s = 0
self.ts_tod_ns = 0
self.ts_tod_updated = False
self.ts_tod_offset_ns = 0
self.ts_tod_alt_s = 0
self.ts_tod_alt_offset_ns = 0
self.td_delay = td_delay
self.timestamp_delay = [(0, 0, 0, 0)]
self.data.setimmediatevalue(1)
self.pps = Event()
self._run_cr = None
self._init_reset(reset, reset_active_level)
def set_period(self, ns, fns):
self.period_ns = int(ns)
self.period_fns = int(fns) & 0xffffffff
def set_drift(self, num, denom):
self.drift_num = int(num)
self.drift_denom = int(denom)
def set_period_ns(self, t):
t = Decimal(t)
period, drift = self.ctx.divmod(Decimal(t) * Decimal(2**32), Decimal(1))
period = int(period)
frac = Fraction(drift).limit_denominator(2**16-1)
self.set_period(period >> 32, period & 0xffffffff)
self.set_drift(frac.numerator, frac.denominator)
self.log.info("Set period: %s ns", t)
self.log.info("Period: 0x%x ns 0x%08x fns", self.period_ns, self.period_fns)
self.log.info("Drift: 0x%04x / 0x%04x fns", self.drift_num, self.drift_denom)
def get_period_ns(self):
p = Decimal((self.period_ns << 32) | self.period_fns)
if self.drift_denom:
p += Decimal(self.drift_num) / Decimal(self.drift_denom)
return p / Decimal(2**32)
def set_ts_tod(self, ts_s, ts_ns, ts_fns):
self.ts_tod_s = int(ts_s)
self.ts_tod_ns = int(ts_ns)
self.ts_fns = int(ts_fns)
self.ts_tod_updated = True
def set_ts_tod_64(self, ts):
ts = int(ts)
self.set_ts_tod(ts >> 48, (ts >> 32) & 0x3fffffff, (ts & 0xffff) << 16)
def set_ts_tod_ns(self, t):
ts_s, ts_ns = self.ctx.divmod(Decimal(t), Decimal(1000000000))
ts_s = ts_s.scaleb(-9).to_integral_value()
ts_ns, ts_fns = self.ctx.divmod(ts_ns, Decimal(1))
ts_ns = ts_ns.to_integral_value()
ts_fns = (ts_fns * Decimal(2**32)).to_integral_value()
self.set_ts_tod(ts_s, ts_ns, ts_fns)
def set_ts_tod_s(self, t):
self.set_ts_tod_ns(Decimal(t).scaleb(9, self.ctx))
def set_ts_tod_sim_time(self):
self.set_ts_tod_ns(Decimal(get_sim_time('fs')).scaleb(-6))
def get_ts_tod(self):
ts_tod_s, ts_tod_ns, ts_rel_ns, ts_fns = self.timestamp_delay[0]
return (ts_tod_s, ts_tod_ns, ts_fns)
def get_ts_tod_96(self):
ts_tod_s, ts_tod_ns, ts_fns = self.get_ts_tod()
return (ts_tod_s << 48) | (ts_tod_ns << 16) | (ts_fns >> 16)
def get_ts_tod_ns(self):
ts_tod_s, ts_tod_ns, ts_fns = self.get_ts_tod()
ns = Decimal(ts_fns) / Decimal(2**32)
ns = self.ctx.add(ns, Decimal(ts_tod_ns))
return self.ctx.add(ns, Decimal(ts_tod_s).scaleb(9))
def get_ts_tod_s(self):
return self.get_ts_tod_ns().scaleb(-9, self.ctx)
def set_ts_rel(self, ts_ns, ts_fns):
self.ts_rel_ns = int(ts_ns)
self.ts_fns = int(ts_fns)
self.ts_rel_updated = True
def set_ts_rel_64(self, ts):
ts = int(ts)
self.set_ts_rel(ts >> 16, (ts & 0xffff) << 16)
def set_ts_rel_ns(self, t):
ts_ns, ts_fns = self.ctx.divmod(Decimal(t), Decimal(1))
ts_ns = ts_ns.to_integral_value()
ts_fns = (ts_fns * Decimal(2**32)).to_integral_value()
self.set_ts_rel(ts_ns, ts_fns)
def set_ts_rel_s(self, t):
self.set_ts_rel_ns(Decimal(t).scaleb(9, self.ctx))
def set_ts_rel_sim_time(self):
self.set_ts_rel_ns(Decimal(get_sim_time('fs')).scaleb(-6))
def get_ts_rel(self):
ts_tod_s, ts_tod_ns, ts_rel_ns, ts_fns = self.timestamp_delay[0]
return (ts_rel_ns, ts_fns)
def get_ts_rel_64(self):
ts_rel_ns, ts_fns = self.get_ts_rel()
return (ts_rel_ns << 16) | (ts_fns >> 16)
def get_ts_rel_ns(self):
ts_rel_ns, ts_fns = self.get_ts_rel()
return self.ctx.add(Decimal(ts_fns) / Decimal(2**32), Decimal(ts_rel_ns))
def get_ts_rel_s(self):
return self.get_ts_rel_ns().scaleb(-9, self.ctx)
def _handle_reset(self, state):
if state:
self.log.info("Reset asserted")
if self._run_cr is not None:
self._run_cr.kill()
self._run_cr = None
self.ts_tod_s = 0
self.ts_tod_ns = 0
self.ts_rel_ns = 0
self.ts_fns = 0
self.drift_cnt = 0
self.data.value = 1
else:
self.log.info("Reset de-asserted")
if self._run_cr is None:
self._run_cr = cocotb.start_soon(self._run())
async def _run(self):
clock_edge_event = RisingEdge(self.clock)
msg_index = 0
msg = None
msg_delay = 0
word = None
bit_index = 0
while True:
await clock_edge_event
# delay timestamp
self.timestamp_delay.append((self.ts_tod_s, self.ts_tod_ns, self.ts_rel_ns, self.ts_fns))
while len(self.timestamp_delay) > 14*17+self.td_delay:
self.timestamp_delay.pop(0)
# increment fns portion
self.ts_fns += ((self.period_ns << 32) + self.period_fns)
if self.drift_denom:
if self.drift_cnt > 0:
self.drift_cnt -= 1
else:
self.drift_cnt = self.drift_denom-1
self.ts_fns += self.drift_num
ns_inc = self.ts_fns >> 32
self.ts_fns &= 0xffffffff
# increment relative timestamp
self.ts_rel_ns = (self.ts_rel_ns + ns_inc) & 0xffffffffffff
# increment ToD timestamp
self.ts_tod_ns = self.ts_tod_ns + ns_inc
if self.ts_tod_ns >= 1000000000:
self.log.info("Seconds rollover")
self.pps.set()
self.ts_tod_s += 1
self.ts_tod_ns -= 1000000000
# compute offset for current second
self.ts_tod_offset_ns = (self.ts_tod_ns - self.ts_rel_ns) & 0xffffffff
# compute alternate offset
if self.ts_tod_ns & (1 << 29):
# latter half of second; compute offset for next second
self.ts_tod_alt_s = self.ts_tod_s+1
self.ts_tod_alt_offset_ns = (self.ts_tod_offset_ns - 1000000000) & 0xffffffff
else:
# former half of second; compute offset for previous second
self.ts_tod_alt_s = self.ts_tod_s-1
self.ts_tod_alt_offset_ns = (self.ts_tod_offset_ns + 1000000000) & 0xffffffff
if msg_delay <= 0:
# build message
msg = []
# word 0: control
ctrl = 0
ctrl |= msg_index & 0xf
ctrl |= bool(self.ts_rel_updated) << 8
ctrl |= bool(self.ts_tod_s & 1) << 9
self.ts_rel_updated = False
msg.append(ctrl)
if msg_index == 0:
# msg 0 word 1: current ToD TS ns 15:0
msg.append(self.ts_tod_ns & 0xffff)
# msg 0 word 2: current ToD TS ns 29:16 and flag bit
msg.append(((self.ts_tod_ns >> 16) & 0x3fff) | (0x8000 if self.ts_tod_updated else 0))
self.ts_tod_updated = False
# msg 0 word 3: current ToD TS seconds 15:0
msg.append(self.ts_tod_s & 0xffff)
# msg 0 word 4: current ToD TS seconds 31:16
msg.append((self.ts_tod_s >> 16) & 0xffff)
# msg 0 word 5: current ToD TS seconds 47:32
msg.append((self.ts_tod_s >> 32) & 0xffff)
msg_index = 1
elif msg_index == 1:
# msg 1 word 1: current ToD TS ns offset 15:0
msg.append(self.ts_tod_offset_ns & 0xffff)
# msg 1 word 2: current ToD TS ns offset 31:16
msg.append((self.ts_tod_offset_ns >> 16) & 0xffff)
# msg 1 word 3: drift num
msg.append(self.drift_num)
# msg 1 word 4: drift denom
msg.append(self.drift_denom)
# msg 1 word 5: drift state
msg.append(self.drift_cnt)
msg_index = 2
elif msg_index == 2:
# msg 2 word 1: alternate ToD TS ns offset 15:0
msg.append(self.ts_tod_alt_offset_ns & 0xffff)
# msg 2 word 2: alternate ToD TS ns offset 31:16
msg.append((self.ts_tod_alt_offset_ns >> 16) & 0xffff)
# msg 2 word 3: alternate ToD TS seconds 15:0
msg.append(self.ts_tod_alt_s & 0xffff)
# msg 2 word 4: alternate ToD TS seconds 31:16
msg.append((self.ts_tod_alt_s >> 16) & 0xffff)
# msg 2 word 5: alternate ToD TS seconds 47:32
msg.append((self.ts_tod_alt_s >> 32) & 0xffff)
msg_index = 0
# word 6: current fns 15:0
msg.append(self.ts_fns & 0xffff)
# word 7: current fns 31:16
msg.append((self.ts_fns >> 16) & 0xffff)
# word 8: current relative TS ns 15:0
msg.append(self.ts_rel_ns & 0xffff)
# word 9: current relative TS ns 31:16
msg.append((self.ts_rel_ns >> 16) & 0xffff)
# word 10: current relative TS ns 47:32
msg.append((self.ts_rel_ns >> 32) & 0xffff)
# word 11: current phase increment fns 15:0
msg.append(self.period_fns & 0xffff)
# word 12: current phase increment fns 31:16
msg.append((self.period_fns >> 16) & 0xffff)
# word 13: current phase increment ns 7:0 + crc
msg.append(self.period_ns & 0xff)
msg_delay = 255
else:
msg_delay -= 1
# serialize message
if word is None:
if msg:
word = msg.pop(0)
bit_index = 0
self.data.value = 0
else:
self.data.value = 1
else:
self.data.value = bool((word >> bit_index) & 1)
bit_index += 1
if bit_index == 16:
word = None
class PtpTdSink(Reset):
def __init__(self,
data=None,
clock=None,
reset=None,
reset_active_level=True,
period_ns=6.4,
td_delay=32,
*args, **kwargs):
self.log = logging.getLogger(f"cocotb.{data._path}")
self.data = data
self.clock = clock
self.reset = reset
self.log.info("PTP time distribution sink")
self.log.info("Copyright (c) 2023 Alex Forencich")
self.log.info("https://github.com/alexforencich/verilog-ethernet")
super().__init__(*args, **kwargs)
self.ctx = Context(prec=60)
self.period_ns = 0
self.period_fns = 0
self.drift_num = 0
self.drift_denom = 0
self.ts_fns = 0
self.ts_rel_ns = 0
self.ts_tod_s = 0
self.ts_tod_ns = 0
self.ts_tod_offset_ns = 0
self.ts_tod_alt_s = 0
self.ts_tod_alt_offset_ns = 0
self.td_delay = td_delay
self.drift_cnt = 0
self.pps = Event()
self._run_cr = None
self._init_reset(reset, reset_active_level)
def get_period_ns(self):
p = Decimal((self.period_ns << 32) | self.period_fns)
if self.drift_denom:
return p + Decimal(self.drift_num) / Decimal(self.drift_denom)
return p / Decimal(2**32)
def get_ts_tod(self):
return (self.ts_tod_s, self.ts_tod_ns, self.ts_fns)
def get_ts_tod_96(self):
ts_tod_s, ts_tod_ns, ts_fns = self.get_ts_tod()
return (ts_tod_s << 48) | (ts_tod_ns << 16) | (ts_fns >> 16)
def get_ts_tod_ns(self):
ts_tod_s, ts_tod_ns, ts_fns = self.get_ts_tod()
ns = Decimal(ts_fns) / Decimal(2**32)
ns = self.ctx.add(ns, Decimal(ts_tod_ns))
return self.ctx.add(ns, Decimal(ts_tod_s).scaleb(9))
def get_ts_tod_s(self):
return self.get_ts_tod_ns().scaleb(-9, self.ctx)
def get_ts_rel(self):
return (self.ts_rel_ns, self.ts_fns)
def get_ts_rel_64(self):
ts_rel_ns, ts_fns = self.get_ts_rel()
return (ts_rel_ns << 16) | (ts_fns >> 16)
def get_ts_rel_ns(self):
ts_rel_ns, ts_fns = self.get_ts_rel()
return self.ctx.add(Decimal(ts_fns) / Decimal(2**32), Decimal(ts_rel_ns))
def get_ts_rel_s(self):
return self.get_ts_rel_ns().scaleb(-9, self.ctx)
def _handle_reset(self, state):
if state:
self.log.info("Reset asserted")
if self._run_cr is not None:
self._run_cr.kill()
self._run_cr = None
self.ts_tod_s = 0
self.ts_tod_ns = 0
self.ts_rel_ns = 0
self.ts_fns = 0
self.drift_cnt = 0
self.data.value = 1
else:
self.log.info("Reset de-asserted")
if self._run_cr is None:
self._run_cr = cocotb.start_soon(self._run())
async def _run(self):
clock_edge_event = RisingEdge(self.clock)
msg_index = 0
msg = None
msg_delay = 0
cur_msg = []
word = None
bit_index = 0
while True:
await clock_edge_event
sdi_sample = self.data.value.integer
# increment fns portion
self.ts_fns += ((self.period_ns << 32) + self.period_fns)
if self.drift_denom:
if self.drift_cnt > 0:
self.drift_cnt -= 1
else:
self.drift_cnt = self.drift_denom-1
self.ts_fns += self.drift_num
ns_inc = self.ts_fns >> 32
self.ts_fns &= 0xffffffff
# increment relative timestamp
self.ts_rel_ns = (self.ts_rel_ns + ns_inc) & 0xffffffffffff
# increment ToD timestamp
self.ts_tod_ns = self.ts_tod_ns + ns_inc
if self.ts_tod_ns >= 1000000000:
self.log.info("Seconds rollover")
self.pps.set()
self.ts_tod_s += 1
self.ts_tod_ns -= 1000000000
# process messages
if msg_delay > 0:
msg_delay -= 1
if msg_delay == 0 and msg:
self.log.info("process message %r", msg)
# word 0: control
msg_index = msg[0] & 0xf
if msg_index == 0:
# msg 0 word 1: current ToD TS ns 15:0
# msg 0 word 2: current ToD TS ns 29:16
val = ((msg[2] & 0x3fff) << 16) | msg[1]
if self.ts_tod_ns != val:
self.log.info("update ts_tod_ns: old 0x%x, new 0x%x", self.ts_tod_ns, val)
self.ts_tod_ns = val
# msg 0 word 3: current ToD TS seconds 15:0
# msg 0 word 4: current ToD TS seconds 31:16
# msg 0 word 5: current ToD TS seconds 47:32
val = (msg[5] << 32) | (msg[4] << 16) | msg[3]
if self.ts_tod_s != val:
self.log.info("update ts_tod_s: old 0x%x, new 0x%x", self.ts_tod_s, val)
self.ts_tod_s = val
elif msg_index == 1:
# msg 1 word 1: current ToD TS ns offset 15:0
# msg 1 word 2: current ToD TS ns offset 31:16
val = (msg[2] << 16) | msg[1]
if self.ts_tod_offset_ns != val:
self.log.info("update ts_tod_offset_ns: old 0x%x, new 0x%x", self.ts_tod_offset_ns, val)
self.ts_tod_offset_ns = val
# msg 1 word 3: drift num
val = msg[3]
if self.drift_num != val:
self.log.info("update drift_num: old 0x%x, new 0x%x", self.drift_num, val)
self.drift_num = val
# msg 1 word 4: drift denom
val = msg[4]
if self.drift_denom != val:
self.log.info("update drift_denom: old 0x%x, new 0x%x", self.drift_denom, val)
self.drift_denom = val
# msg 1 word 5: drift state
val = msg[5]
if self.drift_cnt != val:
self.log.info("update drift_cnt: old 0x%x, new 0x%x", self.drift_cnt, val)
self.drift_cnt = val
elif msg_index == 2:
# msg 2 word 1: alternate ToD TS ns offset 15:0
# msg 2 word 2: alternate ToD TS ns offset 31:16
val = (msg[2] << 16) | msg[1]
if self.ts_tod_alt_offset_ns != val:
self.log.info("update ts_tod_alt_offset_ns: old 0x%x, new 0x%x", self.ts_tod_alt_offset_ns, val)
self.ts_tod_alt_offset_ns = val
# msg 2 word 3: alternate ToD TS seconds 15:0
# msg 2 word 4: alternate ToD TS seconds 31:16
# msg 2 word 5: alternate ToD TS seconds 47:32
val = (msg[5] << 32) | (msg[4] << 16) | msg[3]
if self.ts_tod_alt_s != val:
self.log.info("update ts_tod_alt_s: old 0x%x, new 0x%x", self.ts_tod_alt_s, val)
self.ts_tod_alt_s = val
# word 6: current fns 15:0
# word 7: current fns 31:16
val = (msg[7] << 16) | msg[6]
if self.ts_fns != val:
self.log.info("update ts_fns: old 0x%x, new 0x%x", self.ts_fns, val)
self.ts_fns = val
# word 8: current relative TS ns 15:0
# word 9: current relative TS ns 31:16
# word 10: current relative TS ns 47:32
val = (msg[10] << 32) | (msg[9] << 16) | msg[8]
if self.ts_rel_ns != val:
self.log.info("update ts_rel_ns: old 0x%x, new 0x%x", self.ts_rel_ns, val)
self.ts_rel_ns = val
# word 11: current phase increment fns 15:0
# word 12: current phase increment fns 31:16
val = (msg[12] << 16) | msg[11]
if self.period_fns != val:
self.log.info("update period_fns: old 0x%x, new 0x%x", self.period_fns, val)
self.period_fns = val
# word 13: current phase increment ns 7:0 + crc
val = msg[13] & 0xff
if self.period_ns != val:
self.log.info("update period_ns: old 0x%x, new 0x%x", self.period_ns, val)
self.period_ns = val
msg = None
# deserialize message
if word is not None:
word = word | (sdi_sample << bit_index)
bit_index += 1
if bit_index == 16:
cur_msg.append(word)
word = None
else:
if not sdi_sample:
# start bit
word = 0
bit_index = 0
elif cur_msg:
# idle
msg = cur_msg
msg_delay = self.td_delay
cur_msg = []
Reference in New Issue View Git Blame Copy Permalink