From 9fb450beab3728a49e7c9493f3890588acf3375f Mon Sep 17 00:00:00 2001
From: Alex Forencich <alex@alexforencich.com>
Date: Tue, 7 Nov 2023 12:28:52 -0800
Subject: [PATCH] Use gain scheduling in PTP CDC module for deskew PID loop

Signed-off-by: Alex Forencich <alex@alexforencich.com>
---
 rtl/ptp_clock_cdc.v | 52 ++++++++++++++++++++++++++++++++-------------
 1 file changed, 37 insertions(+), 15 deletions(-)

diff --git a/rtl/ptp_clock_cdc.v b/rtl/ptp_clock_cdc.v
index 13ce0de3b..0c402bbb7 100644
--- a/rtl/ptp_clock_cdc.v
+++ b/rtl/ptp_clock_cdc.v
@@ -86,13 +86,13 @@ localparam CMP_FNS_WIDTH = 4;
 localparam PHASE_CNT_WIDTH = LOG_RATE;
 localparam PHASE_ACC_WIDTH = PHASE_CNT_WIDTH+16;
 
-localparam LOG_SAMPLE_SYNC_RATE = LOG_RATE;
+localparam LOG_SAMPLE_SYNC_RATE = 4;
 localparam SAMPLE_ACC_WIDTH = LOG_SAMPLE_SYNC_RATE+2;
 
 localparam LOG_PHASE_ERR_RATE = 4;
 localparam PHASE_ERR_ACC_WIDTH = LOG_PHASE_ERR_RATE+2;
 
-localparam DEST_SYNC_LOCK_WIDTH = 7;
+localparam DEST_SYNC_LOCK_WIDTH = 5;
 localparam FREQ_LOCK_WIDTH = 8;
 localparam PTP_LOCK_WIDTH = 8;
 
@@ -355,6 +355,8 @@ reg [1:0] dest_ovf;
 reg [DEST_SYNC_LOCK_WIDTH-1:0] dest_sync_lock_count_reg = 0, dest_sync_lock_count_next;
 reg dest_sync_locked_reg = 1'b0, dest_sync_locked_next;
 
+reg dest_gain_sel_reg = 0, dest_gain_sel_next;
+
 always @* begin
     {dest_update_next, dest_phase_next} = dest_phase_reg + dest_phase_inc_reg;
     dest_phase_inc_next = dest_phase_inc_reg;
@@ -364,16 +366,32 @@ always @* begin
     dest_sync_lock_count_next = dest_sync_lock_count_reg;
     dest_sync_locked_next = dest_sync_locked_reg;
 
+    dest_gain_sel_next = dest_gain_sel_reg;
+
     if (sample_acc_sync_valid_reg) begin
         // updated sampled dest_phase error
 
-        // time integral of error
-        if (dest_sync_locked_reg) begin
-            {dest_ovf, dest_err_int_next} = $signed({1'b0, dest_err_int_reg}) + $signed(sample_acc_sync_reg);
+        // gain scheduling
+        if (!sample_acc_sync_reg[SAMPLE_ACC_WIDTH-1]) begin
+            if (sample_acc_sync_reg[SAMPLE_ACC_WIDTH-4 +: 3]) begin
+                dest_gain_sel_next = 1'b1;
+            end else begin
+                dest_gain_sel_next = 1'b0;
+            end
         end else begin
-            {dest_ovf, dest_err_int_next} = $signed({1'b0, dest_err_int_reg}) + ($signed(sample_acc_sync_reg) * 2**6);
+            if (~sample_acc_sync_reg[SAMPLE_ACC_WIDTH-4 +: 3]) begin
+                dest_gain_sel_next = 1'b1;
+            end else begin
+                dest_gain_sel_next = 1'b0;
+            end
         end
 
+        // time integral of error
+        case (dest_gain_sel_reg)
+            1'd0: {dest_ovf, dest_err_int_next} = $signed({1'b0, dest_err_int_reg}) + $signed(sample_acc_sync_reg);
+            1'd1: {dest_ovf, dest_err_int_next} = $signed({1'b0, dest_err_int_reg}) + ($signed(sample_acc_sync_reg) * 2**7);
+        endcase
+
         // saturate
         if (dest_ovf[1]) begin
             // sign bit set indicating underflow across zero; saturate to zero
@@ -384,11 +402,10 @@ always @* begin
         end
 
         // compute output
-        if (dest_sync_locked_reg) begin
-            {dest_ovf, dest_phase_inc_next} = $signed({1'b0, dest_err_int_reg}) + ($signed(sample_acc_sync_reg) * 2**4);
-        end else begin
-            {dest_ovf, dest_phase_inc_next} = $signed({1'b0, dest_err_int_reg}) + ($signed(sample_acc_sync_reg) * 2**10);
-        end
+        case (dest_gain_sel_reg)
+            1'd0: {dest_ovf, dest_phase_inc_next} = $signed({1'b0, dest_err_int_reg}) + ($signed(sample_acc_sync_reg) * 2**4);
+            1'd1: {dest_ovf, dest_phase_inc_next} = $signed({1'b0, dest_err_int_reg}) + ($signed(sample_acc_sync_reg) * 2**11);
+        endcase
 
         // saturate
         if (dest_ovf[1]) begin
@@ -400,15 +417,18 @@ always @* begin
         end
 
         // locked status
-        if ($signed(sample_acc_sync_reg[SAMPLE_ACC_WIDTH-1:2]) == 0 || $signed(sample_acc_sync_reg[SAMPLE_ACC_WIDTH-1:1]) == -1) begin
-            if (dest_sync_lock_count_reg == {DEST_SYNC_LOCK_WIDTH{1'b1}}) begin
+        if (dest_gain_sel_reg == 1'd0) begin
+            if (&dest_sync_lock_count_reg) begin
                 dest_sync_locked_next = 1'b1;
             end else begin
                 dest_sync_lock_count_next = dest_sync_lock_count_reg + 1;
             end
         end else begin
-            dest_sync_lock_count_next = 0;
-            dest_sync_locked_next = 1'b0;
+            if (|dest_sync_lock_count_reg) begin
+                dest_sync_lock_count_next = dest_sync_lock_count_reg - 1;
+            end else begin
+                dest_sync_locked_next = 1'b0;
+            end
         end
     end
 end
@@ -510,6 +530,8 @@ always @(posedge output_clk) begin
     dest_sync_lock_count_reg <= dest_sync_lock_count_next;
     dest_sync_locked_reg <= dest_sync_locked_next;
 
+    dest_gain_sel_reg <= dest_gain_sel_next;
+
     if (output_rst) begin
         dest_phase_reg <= {PHASE_ACC_WIDTH{1'b0}};
         dest_phase_inc_reg <= {PHASE_ACC_WIDTH{1'b0}};