From a2b1228d3cef4523fab0914ea99bdfbac730eac0 Mon Sep 17 00:00:00 2001
From: Alex Forencich <alex@alexforencich.com>
Date: Sun, 6 Dec 2020 00:39:28 -0800
Subject: [PATCH] Add set_period, set_drift, set_period_ns, and get_period_ns
 methods to PtpClock

---
 cocotbext/eth/ptp.py | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/cocotbext/eth/ptp.py b/cocotbext/eth/ptp.py
index 3fb0606..0711ada 100644
--- a/cocotbext/eth/ptp.py
+++ b/cocotbext/eth/ptp.py
@@ -24,6 +24,7 @@ THE SOFTWARE.
 
 import logging
 import math
+from fractions import Fraction
 
 import cocotb
 from cocotb.triggers import RisingEdge, ReadOnly
@@ -91,6 +92,33 @@ class PtpClock(object):
 
         cocotb.fork(self._run())
 
+    def set_period(self, ns, fns):
+        self.period_ns = int(ns)
+        self.period_fns = int(fns) & 0xffff
+
+    def set_drift(self, ns, fns, rate):
+        self.drift_ns = int(ns)
+        self.drift_fns = int(fns) & 0xffff
+        self.drift_rate = int(rate)
+
+    def set_period_ns(self, t):
+        drift, period = math.modf(t*2**16)
+        period = int(period)
+        frac = Fraction(drift).limit_denominator(2**16)
+        drift = frac.numerator
+        rate = frac.denominator
+        self.period_ns = period >> 16
+        self.period_fns = period & 0xffff
+        self.drift_ns = drift >> 16
+        self.drift_fns = drift & 0xffff
+        self.drift_rate = rate
+
+    def get_period_ns(self):
+        p = ((self.period_ns << 16) | self.period_fns) / 2**16
+        if self.drift_rate:
+            return p + ((self.drift_ns << 16) | self.drift_fns) / self.drift_rate / 2**16
+        return p
+
     def set_ts_96(self, ts_s, ts_ns=None, ts_fns=None):
         ts_s = int(ts_s)
         if ts_fns is not None: