diff --git a/rtl/ptp_clock.v b/rtl/ptp_clock.v
new file mode 100644
index 00000000..f1f27d64
--- /dev/null
+++ b/rtl/ptp_clock.v
@@ -0,0 +1,246 @@
+/*
+
+Copyright (c) 2015-2019 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.
+
+*/
+
+// Language: Verilog 2001
+
+`timescale 1ns / 1ps
+
+/*
+ * PTP clock module
+ */
+module ptp_clock #
+(
+    parameter PERIOD_NS_WIDTH = 4,
+    parameter OFFSET_NS_WIDTH = 4,
+    parameter DRIFT_NS_WIDTH = 4,
+    parameter FNS_WIDTH = 16,
+    parameter PERIOD_NS = 4'h6,
+    parameter PERIOD_FNS = 16'h6666,
+    parameter DRIFT_ENABLE = 1,
+    parameter DRIFT_NS = 4'h0,
+    parameter DRIFT_FNS = 16'h0002,
+    parameter DRIFT_RATE = 16'h0005
+)
+(
+    input  wire                       clk,
+    input  wire                       rst,
+
+    /*
+     * Timestamp inputs for synchronization
+     */
+    input  wire [95:0]                input_ts_96,
+    input  wire                       input_ts_96_valid,
+    input  wire [63:0]                input_ts_64,
+    input  wire                       input_ts_64_valid,
+
+    /*
+     * Period adjustment
+     */
+    input  wire [PERIOD_NS_WIDTH-1:0] input_period_ns,
+    input  wire [FNS_WIDTH-1:0]       input_period_fns,
+    input  wire                       input_period_valid,
+
+    /*
+     * Offset adjustment
+     */
+    input  wire [OFFSET_NS_WIDTH-1:0] input_adj_ns,
+    input  wire [FNS_WIDTH-1:0]       input_adj_fns,
+    input  wire [15:0]                input_adj_count,
+    input  wire                       input_adj_valid,
+    output wire                       input_adj_active,
+
+    /*
+     * Drift adjustment
+     */
+    input  wire [DRIFT_NS_WIDTH-1:0]  input_drift_ns,
+    input  wire [FNS_WIDTH-1:0]       input_drift_fns,
+    input  wire [15:0]                input_drift_rate,
+    input  wire                       input_drift_valid,
+
+    /*
+     * Timestamp outputs
+     */
+    output wire [95:0]                output_ts_96,
+    output wire [63:0]                output_ts_64,
+    output wire                       output_ts_step,
+
+    /*
+     * PPS output
+     */
+    output wire                       output_pps
+);
+
+parameter INC_NS_WIDTH = $clog2(2**PERIOD_NS_WIDTH + 2**OFFSET_NS_WIDTH + 2**DRIFT_NS_WIDTH);
+
+reg [PERIOD_NS_WIDTH-1:0] period_ns_reg = PERIOD_NS;
+reg [FNS_WIDTH-1:0] period_fns_reg = PERIOD_FNS;
+
+reg [OFFSET_NS_WIDTH-1:0] adj_ns_reg = 0;
+reg [FNS_WIDTH-1:0] adj_fns_reg = 0;
+reg [15:0] adj_count_reg = 0;
+reg adj_active_reg = 0;
+
+reg [DRIFT_NS_WIDTH-1:0] drift_ns_reg = DRIFT_NS;
+reg [FNS_WIDTH-1:0] drift_fns_reg = DRIFT_FNS;
+reg [15:0] drift_rate_reg = DRIFT_RATE;
+
+reg [INC_NS_WIDTH-1:0] ts_inc_ns_reg = 0;
+reg [FNS_WIDTH-1:0] ts_inc_fns_reg = 0;
+
+reg [47:0] ts_96_s_reg = 0;
+reg [29:0] ts_96_ns_reg = 0;
+reg [FNS_WIDTH-1:0] ts_96_fns_reg = 0;
+reg [29:0] ts_96_ns_inc_reg = 0;
+reg [FNS_WIDTH-1:0] ts_96_fns_inc_reg = 0;
+reg [30:0] ts_96_ns_ovf_reg = 31'h7fffffff;
+reg [FNS_WIDTH-1:0] ts_96_fns_ovf_reg = 16'hffff;
+
+reg [47:0] ts_64_ns_reg = 0;
+reg [FNS_WIDTH-1:0] ts_64_fns_reg = 0;
+
+reg ts_step_reg = 1'b0;
+
+reg [15:0] drift_cnt = 0;
+
+reg [47:0] temp;
+
+reg pps_reg = 0;
+
+assign input_adj_active = adj_active_reg;
+
+assign output_ts_96[95:48] = ts_96_s_reg;
+assign output_ts_96[47:46] = 2'b00;
+assign output_ts_96[45:16] = ts_96_ns_reg;
+assign output_ts_96[15:0]  = FNS_WIDTH > 16 ? ts_96_fns_reg >> (FNS_WIDTH-16) : ts_96_fns_reg << (16-FNS_WIDTH);
+assign output_ts_64[63:16] = ts_64_ns_reg;
+assign output_ts_64[15:0]  = FNS_WIDTH > 16 ? ts_64_fns_reg >> (FNS_WIDTH-16) : ts_64_fns_reg << (16-FNS_WIDTH);
+assign output_ts_step = ts_step_reg;
+
+assign output_pps = pps_reg;
+
+always @(posedge clk) begin
+    ts_step_reg <= 0;
+
+    // latch parameters
+    if (input_period_valid) begin
+        period_ns_reg <= input_period_ns;
+        period_fns_reg <= input_period_fns;
+    end
+
+    if (input_adj_valid) begin
+        adj_ns_reg <= input_adj_ns;
+        adj_fns_reg <= input_adj_fns;
+        adj_count_reg <= input_adj_count;
+    end
+
+    if (DRIFT_ENABLE && input_drift_valid) begin
+        drift_ns_reg <= input_drift_ns;
+        drift_fns_reg <= input_drift_fns;
+        drift_rate_reg <= input_drift_rate;
+    end
+
+    // timestamp increment calculation
+    {ts_inc_ns_reg, ts_inc_fns_reg} <= $signed({period_ns_reg, period_fns_reg}) +
+        (adj_active_reg ? $signed({adj_ns_reg, adj_fns_reg}) : 0) +
+        ((DRIFT_ENABLE && drift_cnt == 0) ? $signed({drift_ns_reg, drift_fns_reg}) : 0);
+
+    // offset adjust counter
+    if (adj_count_reg > 0) begin
+        adj_count_reg <= adj_count_reg - 1;
+        adj_active_reg <= 1;
+        ts_step_reg <= 1;
+    end else begin
+        adj_active_reg <= 0;
+    end
+
+    // drift counter
+    if (drift_cnt == 0) begin
+        drift_cnt <= drift_rate_reg-1;
+    end else begin
+        drift_cnt <= drift_cnt - 1;
+    end
+
+    // 96 bit timestamp
+    if (input_ts_96_valid) begin
+        // load timestamp
+        {ts_96_ns_inc_reg, ts_96_fns_inc_reg} <= (FNS_WIDTH > 16 ? input_ts_96[45:0] << (FNS_WIDTH-16) : input_ts_96[45:0] >> (16-FNS_WIDTH)) + {ts_inc_ns_reg, ts_inc_fns_reg};
+        {ts_96_ns_ovf_reg, ts_96_fns_ovf_reg} <= (FNS_WIDTH > 16 ? input_ts_96[45:0] << (FNS_WIDTH-16) : input_ts_96[45:0] >> (16-FNS_WIDTH)) + {ts_inc_ns_reg, ts_inc_fns_reg} - {31'd1_000_000_000, {FNS_WIDTH{1'b0}}};
+        ts_96_s_reg <= input_ts_96[95:48];
+        ts_96_ns_reg <= input_ts_96[45:16];
+        ts_96_fns_reg <= FNS_WIDTH > 16 ? input_ts_96[15:0] << (FNS_WIDTH-16) : input_ts_96[15:0] >> (16-FNS_WIDTH);
+        ts_step_reg <= 1;
+    end else if (!ts_96_ns_ovf_reg[30]) begin
+        // if the overflow lookahead did not borrow, one second has elapsed
+        // increment seconds field, pre-compute both normal increment and overflow values
+        {ts_96_ns_inc_reg, ts_96_fns_inc_reg} <= {ts_96_ns_ovf_reg, ts_96_fns_ovf_reg} + {ts_inc_ns_reg, ts_inc_fns_reg};
+        {ts_96_ns_ovf_reg, ts_96_fns_ovf_reg} <= {ts_96_ns_ovf_reg, ts_96_fns_ovf_reg} + {ts_inc_ns_reg, ts_inc_fns_reg} - {31'd1_000_000_000, {FNS_WIDTH{1'b0}}};
+        {ts_96_ns_reg, ts_96_fns_reg} <= {ts_96_ns_ovf_reg, ts_96_fns_ovf_reg};
+        ts_96_s_reg <= ts_96_s_reg + 1;
+    end else begin
+        // no increment seconds field, pre-compute both normal increment and overflow values
+        {ts_96_ns_inc_reg, ts_96_fns_inc_reg} <= {ts_96_ns_inc_reg, ts_96_fns_inc_reg} + {ts_inc_ns_reg, ts_inc_fns_reg};
+        {ts_96_ns_ovf_reg, ts_96_fns_ovf_reg} <= {ts_96_ns_inc_reg, ts_96_fns_inc_reg} + {ts_inc_ns_reg, ts_inc_fns_reg} - {31'd1_000_000_000, {FNS_WIDTH{1'b0}}};
+        {ts_96_ns_reg, ts_96_fns_reg} <= {ts_96_ns_inc_reg, ts_96_fns_inc_reg};
+        ts_96_s_reg <= ts_96_s_reg;
+    end
+
+    // 64 bit timestamp
+    if (input_ts_64_valid) begin
+        // load timestamp
+        {ts_64_ns_reg, ts_64_fns_reg} <= input_ts_64;
+        ts_step_reg <= 1;
+    end else begin
+        {ts_64_ns_reg, ts_64_fns_reg} <= {ts_64_ns_reg, ts_64_fns_reg} + {ts_inc_ns_reg, ts_inc_fns_reg};
+    end
+
+    pps_reg <= !ts_96_ns_ovf_reg[30];
+
+    if (rst) begin
+        period_ns_reg <= PERIOD_NS;
+        period_fns_reg <= PERIOD_FNS;
+        adj_ns_reg <= 0;
+        adj_fns_reg <= 0;
+        adj_count_reg <= 0;
+        adj_active_reg <= 0;
+        drift_ns_reg <= DRIFT_NS;
+        drift_fns_reg <= DRIFT_FNS;
+        drift_rate_reg <= DRIFT_RATE;
+        ts_inc_ns_reg <= 0;
+        ts_inc_fns_reg <= 0;
+        ts_96_s_reg <= 0;
+        ts_96_ns_reg <= 0;
+        ts_96_fns_reg <= 0;
+        ts_96_ns_inc_reg <= 0;
+        ts_96_fns_inc_reg <= 0;
+        ts_96_ns_ovf_reg <= 31'h7fffffff;
+        ts_96_fns_ovf_reg <= {FNS_WIDTH{1'b1}};
+        ts_64_ns_reg <= 0;
+        ts_64_fns_reg <= 0;
+        ts_step_reg <= 0;
+        drift_cnt <= 0;
+        pps_reg <= 0;
+    end
+end
+
+endmodule
diff --git a/tb/test_ptp_clock.py b/tb/test_ptp_clock.py
new file mode 100755
index 00000000..462c5830
--- /dev/null
+++ b/tb/test_ptp_clock.py
@@ -0,0 +1,394 @@
+#!/usr/bin/env python
+"""
+
+Copyright (c) 2015-2019 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.
+
+"""
+
+from myhdl import *
+import os
+
+module = 'ptp_clock'
+testbench = 'test_%s' % module
+
+srcs = []
+
+srcs.append("../rtl/%s.v" % module)
+srcs.append("%s.v" % testbench)
+
+src = ' '.join(srcs)
+
+build_cmd = "iverilog -o %s.vvp %s" % (testbench, src)
+
+def bench():
+
+    # Parameters
+    PERIOD_NS_WIDTH = 4
+    OFFSET_NS_WIDTH = 4
+    DRIFT_NS_WIDTH = 4
+    FNS_WIDTH = 16
+    PERIOD_NS = 0x6
+    PERIOD_FNS = 0x6666
+    DRIFT_ENABLE = 1
+    DRIFT_NS = 0x0
+    DRIFT_FNS = 0x0002
+    DRIFT_RATE = 0x0005
+
+    # Inputs
+    clk = Signal(bool(0))
+    rst = Signal(bool(0))
+    current_test = Signal(intbv(0)[8:])
+
+    input_ts_96 = Signal(intbv(0)[96:])
+    input_ts_96_valid = Signal(bool(0))
+    input_ts_64 = Signal(intbv(0)[64:])
+    input_ts_64_valid = Signal(bool(0))
+    input_period_ns = Signal(intbv(0)[PERIOD_NS_WIDTH:])
+    input_period_fns = Signal(intbv(0)[FNS_WIDTH:])
+    input_period_valid = Signal(bool(0))
+    input_adj_ns = Signal(intbv(0)[OFFSET_NS_WIDTH:])
+    input_adj_fns = Signal(intbv(0)[FNS_WIDTH:])
+    input_adj_count = Signal(intbv(0)[16:])
+    input_adj_valid = Signal(bool(0))
+    input_drift_ns = Signal(intbv(0)[DRIFT_NS_WIDTH:])
+    input_drift_fns = Signal(intbv(0)[FNS_WIDTH:])
+    input_drift_rate = Signal(intbv(0)[16:])
+    input_drift_valid = Signal(bool(0))
+
+    # Outputs
+    input_adj_active = Signal(bool(0))
+    output_ts_96 = Signal(intbv(0)[96:])
+    output_ts_64 = Signal(intbv(0)[64:])
+    output_ts_step = Signal(bool(0))
+    output_pps = Signal(bool(0))
+
+    # DUT
+    if os.system(build_cmd):
+        raise Exception("Error running build command")
+
+    dut = Cosimulation(
+        "vvp -m myhdl %s.vvp -lxt2" % testbench,
+        clk=clk,
+        rst=rst,
+        current_test=current_test,
+
+        input_ts_96=input_ts_96,
+        input_ts_96_valid=input_ts_96_valid,
+        input_ts_64=input_ts_64,
+        input_ts_64_valid=input_ts_64_valid,
+
+        input_period_ns=input_period_ns,
+        input_period_fns=input_period_fns,
+        input_period_valid=input_period_valid,
+
+        input_adj_ns=input_adj_ns,
+        input_adj_fns=input_adj_fns,
+        input_adj_count=input_adj_count,
+        input_adj_valid=input_adj_valid,
+        input_adj_active=input_adj_active,
+
+        input_drift_ns=input_drift_ns,
+        input_drift_fns=input_drift_fns,
+        input_drift_rate=input_drift_rate,
+        input_drift_valid=input_drift_valid,
+
+        output_ts_96=output_ts_96,
+        output_ts_64=output_ts_64,
+        output_ts_step=output_ts_step,
+
+        output_pps=output_pps
+    )
+
+    @always(delay(3200))
+    def clkgen():
+        clk.next = not clk
+
+    @instance
+    def check():
+        yield delay(100000)
+        yield clk.posedge
+        rst.next = 1
+        yield clk.posedge
+        rst.next = 0
+        yield clk.posedge
+        yield delay(100000)
+        yield clk.posedge
+
+        # testbench stimulus
+
+        yield clk.posedge
+        print("test 1: Default rate and drift")
+        current_test.next = 1
+
+        yield clk.posedge
+        start_time = now()*1e-12
+        start_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        start_ts_64 = output_ts_64/2**16*1e-9
+
+        for i in range(10000):
+            yield clk.posedge
+
+        stop_time = now()*1e-12
+        stop_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        stop_ts_64 = output_ts_64/2**16*1e-9
+
+        print(stop_time-start_time)
+        print(stop_ts_96-start_ts_96)
+        print(stop_ts_64-start_ts_64)
+
+        assert abs((stop_time-start_time) - (stop_ts_96-start_ts_96)) < 1e-12
+        assert abs((stop_time-start_time) - (stop_ts_64-start_ts_64)) < 1e-12
+
+        yield delay(100000)
+
+        yield clk.posedge
+        print("test 2: Load timestamps")
+        current_test.next = 2
+
+        input_ts_96.next = 12345678
+        input_ts_96_valid.next = 1
+        input_ts_64.next = 87654321
+        input_ts_64_valid.next = 1
+
+        yield clk.posedge
+
+        input_ts_96_valid.next = 0
+        input_ts_64_valid.next = 0
+
+        yield clk.posedge
+
+        assert output_ts_96 == 12345678
+        assert output_ts_64 == 87654321
+        
+        start_time = now()*1e-12
+        start_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        start_ts_64 = output_ts_64/2**16*1e-9
+
+        for i in range(2000):
+            yield clk.posedge
+
+        stop_time = now()*1e-12
+        stop_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        stop_ts_64 = output_ts_64/2**16*1e-9
+
+        print(stop_time-start_time)
+        print(stop_ts_96-start_ts_96)
+        print(stop_ts_64-start_ts_64)
+
+        assert abs((stop_time-start_time) - (stop_ts_96-start_ts_96)) < 1e-12
+        assert abs((stop_time-start_time) - (stop_ts_64-start_ts_64)) < 1e-12
+
+        yield delay(100000)
+
+        yield clk.posedge
+        print("test 3: Seconds increment")
+        current_test.next = 3
+
+        input_ts_96.next = 999990000*2**16
+        input_ts_96_valid.next = 1
+        input_ts_64.next = 999990000*2**16
+        input_ts_64_valid.next = 1
+
+        yield clk.posedge
+
+        input_ts_96_valid.next = 0
+        input_ts_64_valid.next = 0
+
+        yield clk.posedge
+
+        assert output_ts_96 == 999990000*2**16
+        assert output_ts_64 == 999990000*2**16
+        
+        start_time = now()*1e-12
+        start_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        start_ts_64 = output_ts_64/2**16*1e-9
+
+        for i in range(3000):
+            yield clk.posedge
+
+            if output_pps:
+                assert output_ts_96[96:48] == 1
+                assert output_ts_96[48:0] < 10*2**16
+
+        stop_time = now()*1e-12
+        stop_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        stop_ts_64 = output_ts_64/2**16*1e-9
+
+        print(stop_time-start_time)
+        print(stop_ts_96-start_ts_96)
+        print(stop_ts_64-start_ts_64)
+
+        assert abs((stop_time-start_time) - (stop_ts_96-start_ts_96)) < 1e-12
+        assert abs((stop_time-start_time) - (stop_ts_64-start_ts_64)) < 1e-12
+
+        yield delay(100000)
+
+        yield clk.posedge
+        print("test 4: Offset adjust")
+        current_test.next = 4
+
+        input_ts_96.next = 0
+        input_ts_96_valid.next = 1
+        input_ts_64.next = 0
+        input_ts_64_valid.next = 1
+
+        yield clk.posedge
+
+        input_ts_96_valid.next = 0
+        input_ts_64_valid.next = 0
+
+        yield clk.posedge
+
+        start_time = now()*1e-12
+        start_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        start_ts_64 = output_ts_64/2**16*1e-9
+
+        for i in range(2000):
+            yield clk.posedge        
+
+        # 1 ns offset - 1024*64/65536 = 1
+        input_adj_ns.next = 0
+        input_adj_fns.next = 64
+        input_adj_count.next = 1024
+        input_adj_valid.next = 1
+
+        for i in range(2000):
+            yield clk.posedge
+            input_adj_valid.next = 0
+
+        stop_time = now()*1e-12
+        stop_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        stop_ts_64 = output_ts_64/2**16*1e-9
+
+        print(stop_time-start_time)
+        print(stop_ts_96-start_ts_96)
+        print(stop_ts_64-start_ts_64)
+
+        assert abs((stop_time-start_time) - (stop_ts_96-start_ts_96) + 1e-9) < 1e-12
+        assert abs((stop_time-start_time) - (stop_ts_64-start_ts_64) + 1e-9) < 1e-12
+
+        yield delay(100000)
+
+        yield clk.posedge
+        print("test 5: Frequency adjust")
+        current_test.next = 5
+
+        input_ts_96.next = 0
+        input_ts_96_valid.next = 1
+        input_ts_64.next = 0
+        input_ts_64_valid.next = 1
+
+        input_period_ns.next = 6
+        input_period_fns.next = 0x6624
+        input_period_valid.next = 1
+
+        # flush old period out of pipeline registers
+        yield clk.posedge
+        yield clk.posedge
+        yield clk.posedge
+
+        input_ts_96_valid.next = 0
+        input_ts_64_valid.next = 0
+        input_period_valid.next = 0
+
+        yield clk.posedge
+        
+        start_time = now()*1e-12
+        start_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        start_ts_64 = output_ts_64/2**16*1e-9
+
+        for i in range(10000):
+            yield clk.posedge
+
+        stop_time = now()*1e-12
+        stop_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        stop_ts_64 = output_ts_64/2**16*1e-9
+
+        print(stop_time-start_time)
+        print(stop_ts_96-start_ts_96)
+        print(stop_ts_64-start_ts_64)
+
+        assert abs((stop_time-start_time) - (stop_ts_96-start_ts_96) * 6.4/(6+(0x6624+2/5)/2**16)) < 1e-12
+        assert abs((stop_time-start_time) - (stop_ts_64-start_ts_64) * 6.4/(6+(0x6624+2/5)/2**16)) < 1e-12
+
+        yield delay(100000)
+
+        yield clk.posedge
+        print("test 6: Drift adjust")
+        current_test.next = 6
+
+        input_ts_96.next = 0
+        input_ts_96_valid.next = 1
+        input_ts_64.next = 0
+        input_ts_64_valid.next = 1
+
+        input_period_ns.next = 6
+        input_period_fns.next = 0x6666
+        input_period_valid.next = 1
+
+        input_drift_ns.next = 0
+        input_drift_fns.next = 20
+        input_drift_rate.next = 5
+        input_drift_valid.next = 1
+
+        # flush old period out of pipeline registers
+        yield clk.posedge
+        yield clk.posedge
+        yield clk.posedge
+
+        input_ts_96_valid.next = 0
+        input_ts_64_valid.next = 0
+        input_period_valid.next = 0
+        input_drift_valid.next = 0
+
+        yield clk.posedge
+        
+        start_time = now()*1e-12
+        start_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        start_ts_64 = output_ts_64/2**16*1e-9
+
+        for i in range(10000):
+            yield clk.posedge
+
+        stop_time = now()*1e-12
+        stop_ts_96 = output_ts_96[96:48] + (output_ts_96[48:0]/2**16*1e-9)
+        stop_ts_64 = output_ts_64/2**16*1e-9
+
+        print(stop_time-start_time)
+        print(stop_ts_96-start_ts_96)
+        print(stop_ts_64-start_ts_64)
+
+        assert abs((stop_time-start_time) - (stop_ts_96-start_ts_96) * 6.4/(6+(0x6666+20/5)/2**16)) < 1e-12
+        assert abs((stop_time-start_time) - (stop_ts_64-start_ts_64) * 6.4/(6+(0x6666+20/5)/2**16)) < 1e-12
+
+        yield delay(100000)
+
+        raise StopSimulation
+
+    return dut, clkgen, check
+
+def test_bench():
+    sim = Simulation(bench())
+    sim.run()
+
+if __name__ == '__main__':
+    print("Running test...")
+    test_bench()
diff --git a/tb/test_ptp_clock.v b/tb/test_ptp_clock.v
new file mode 100644
index 00000000..a95da483
--- /dev/null
+++ b/tb/test_ptp_clock.v
@@ -0,0 +1,146 @@
+/*
+
+Copyright (c) 2015-2019 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.
+
+*/
+
+// Language: Verilog 2001
+
+`timescale 1ps / 1fs
+
+/*
+ * Testbench for ptp_clock
+ */
+module test_ptp_clock;
+
+// Parameters
+parameter PERIOD_NS_WIDTH = 4;
+parameter OFFSET_NS_WIDTH = 4;
+parameter DRIFT_NS_WIDTH = 4;
+parameter FNS_WIDTH = 16;
+parameter PERIOD_NS = 4'h6;
+parameter PERIOD_FNS = 16'h6666;
+parameter DRIFT_ENABLE = 1;
+parameter DRIFT_NS = 4'h0;
+parameter DRIFT_FNS = 16'h0002;
+parameter DRIFT_RATE = 16'h0005;
+
+// Inputs
+reg clk = 0;
+reg rst = 0;
+reg [7:0] current_test = 0;
+
+reg [95:0] input_ts_96 = 0;
+reg input_ts_96_valid = 0;
+reg [63:0] input_ts_64 = 0;
+reg input_ts_64_valid = 0;
+reg [PERIOD_NS_WIDTH-1:0] input_period_ns = 0;
+reg [FNS_WIDTH-1:0] input_period_fns = 0;
+reg input_period_valid = 0;
+reg [OFFSET_NS_WIDTH-1:0] input_adj_ns = 0;
+reg [FNS_WIDTH-1:0] input_adj_fns = 0;
+reg [15:0] input_adj_count = 0;
+reg input_adj_valid = 0;
+reg [DRIFT_NS_WIDTH-1:0] input_drift_ns = 0;
+reg [FNS_WIDTH-1:0] input_drift_fns = 0;
+reg [15:0] input_drift_rate = 0;
+reg input_drift_valid = 0;
+
+// Outputs
+wire input_adj_active;
+wire [95:0] output_ts_96;
+wire [63:0] output_ts_64;
+wire output_ts_step;
+wire output_pps;
+
+initial begin
+    // myhdl integration
+    $from_myhdl(
+        clk,
+        rst,
+        current_test,
+        input_ts_96,
+        input_ts_96_valid,
+        input_ts_64,
+        input_ts_64_valid,
+        input_period_ns,
+        input_period_fns,
+        input_period_valid,
+        input_adj_ns,
+        input_adj_fns,
+        input_adj_count,
+        input_adj_valid,
+        input_drift_ns,
+        input_drift_fns,
+        input_drift_rate,
+        input_drift_valid
+    );
+    $to_myhdl(
+        input_adj_active,
+        output_ts_96,
+        output_ts_64,
+        output_ts_step,
+        output_pps
+    );
+
+    // dump file
+    $dumpfile("test_ptp_clock.lxt");
+    $dumpvars(0, test_ptp_clock);
+end
+
+ptp_clock #(
+    .PERIOD_NS_WIDTH(PERIOD_NS_WIDTH),
+    .OFFSET_NS_WIDTH(OFFSET_NS_WIDTH),
+    .DRIFT_NS_WIDTH (DRIFT_NS_WIDTH),
+    .FNS_WIDTH(FNS_WIDTH),
+    .PERIOD_NS(PERIOD_NS),
+    .PERIOD_FNS(PERIOD_FNS),
+    .DRIFT_ENABLE(DRIFT_ENABLE),
+    .DRIFT_NS(DRIFT_NS),
+    .DRIFT_FNS(DRIFT_FNS),
+    .DRIFT_RATE(DRIFT_RATE)
+)
+UUT (
+    .clk(clk),
+    .rst(rst),
+    .input_ts_96(input_ts_96),
+    .input_ts_96_valid(input_ts_96_valid),
+    .input_ts_64(input_ts_64),
+    .input_ts_64_valid(input_ts_64_valid),
+    .input_period_ns(input_period_ns),
+    .input_period_fns(input_period_fns),
+    .input_period_valid(input_period_valid),
+    .input_adj_ns(input_adj_ns),
+    .input_adj_fns(input_adj_fns),
+    .input_adj_count(input_adj_count),
+    .input_adj_valid(input_adj_valid),
+    .input_adj_active(input_adj_active),
+    .input_drift_ns(input_drift_ns),
+    .input_drift_fns(input_drift_fns),
+    .input_drift_rate(input_drift_rate),
+    .input_drift_valid(input_drift_valid),
+    .output_ts_96(output_ts_96),
+    .output_ts_64(output_ts_64),
+    .output_ts_step(output_ts_step),
+    .output_pps(output_pps)
+);
+
+endmodule