The Servo — Offset, Delay and Correction

// -----------------------------------------------------------------------
// servo_pkg -- shared types for the PTP servo and transparent clock.
// -----------------------------------------------------------------------
package servo_pkg;

  localparam int NS_W   = 32;
  localparam int FRAC_W = 16;        // fractional nanoseconds

  // The offset and the path delay, signed nanoseconds.
  typedef logic signed [47:0] off_t;

  // A rate correction, in parts per billion, signed. 16.1 section 3's
  // increment is trimmed by this.
  typedef logic signed [31:0] ppb_t;

  // correctionField's format: signed 16.16 fixed point nanoseconds.
  typedef logic signed [63:0] correction_t;

  typedef enum logic [2:0] {
    S_FREERUN,     // no reference ever seen
    S_ACQUIRE,     // a reference is present, converging
    S_LOCKED,      // inside the lock window for long enough
    S_HOLDOVER,    // the reference went away; apply the last drift
    S_FAULT        // the reference is present and unusable
  } servo_state_e;

endpackage

// -----------------------------------------------------------------------
// offset_filter -- rejects outliers and produces the error signal the
// controller acts on.
//
// A median-of-N is used rather than a mean, because the outliers a
// network produces are ONE-SIDED: 14.1's queueing only ever DELAYS a
// message, so a contaminated sample is always too large. A mean is
// dragged by them; a median is not.
// -----------------------------------------------------------------------
module offset_filter
  import servo_pkg::*;
#(
  parameter int N = 5,                       // odd, so the median exists
  parameter int REJECT_NS = 100_000          // 100 us: obviously wrong
)(
  input  logic  clk,
  input  logic  rst_n,

  input  logic  sample_valid,
  input  off_t  offset_ns,
  input  off_t  path_ns,
  input  logic  sample_trustworthy,          // 16.2 section 15

  output logic  err_valid,
  output off_t  err_ns,                      // the controller's input
  output off_t  spread_ns,
  output logic [31:0] c_accepted,
  output logic [31:0] c_rejected_gross,
  output logic [31:0] c_rejected_untrusted
);

  off_t win [N];
  logic [$clog2(N+1)-1:0] fill;
  logic [$clog2(N)-1:0]   wp;

  // A selection network for the median. For N = 5 this is a fixed
  // sorting network -- 9 compare-exchanges -- and it is combinational.
  function automatic off_t median5(input off_t a0, a1, a2, a3, a4);
    off_t b [5];
    int i, j;
    off_t t;
    begin
      b[0]=a0; b[1]=a1; b[2]=a2; b[3]=a3; b[4]=a4;
      for (i = 0; i < 5; i++)
        for (j = i+1; j < 5; j++)
          if (b[j] < b[i]) begin t = b[i]; b[i] = b[j]; b[j] = t; end
      median5 = b[2];
    end
  endfunction

  always_ff @(posedge clk or negedge rst_n) begin
    int i;
    if (!rst_n) begin
      for (i = 0; i < N; i++) win[i] <= '0;
      fill <= '0; wp <= '0;
      err_valid <= 1'b0; err_ns <= '0; spread_ns <= '0;
      c_accepted <= '0; c_rejected_gross <= '0;
      c_rejected_untrusted <= '0;
    end else begin
      err_valid <= 1'b0;

      if (sample_valid) begin
        // Gate 1 -- 16.2 section 15's trustworthiness. A sample from an
        // exchange with a negative round trip or a cross-master pairing
        // is not an outlier, it is garbage.
        if (!sample_trustworthy) begin
          c_rejected_untrusted <= c_rejected_untrusted + 1;
        end
        // Gate 2 -- grossly wrong. A 100 us offset on a locked loop is
        // a lost message or a master change, not a measurement.
        else if ((offset_ns > off_t'(REJECT_NS)) ||
                 (offset_ns < -off_t'(REJECT_NS))) begin
          c_rejected_gross <= c_rejected_gross + 1;
        end
        else begin
          automatic off_t mx, mn;
          win[wp] <= offset_ns;
          wp      <= (wp == N-1) ? '0 : (wp + 1'b1);
          if (fill != N) fill <= fill + 1'b1;
          c_accepted <= c_accepted + 1;

          if (fill == N) begin
            err_valid <= 1'b1;
            err_ns    <= median5(win[0], win[1], win[2], win[3], win[4]);
            mx = win[0]; mn = win[0];
            for (i = 1; i < N; i++) begin
              if (win[i] > mx) mx = win[i];
              if (win[i] < mn) mn = win[i];
            end
            spread_ns <= mx - mn;
          end
        end
      end
    end
  end

endmodule

total(T) = σ / √(rate × T)  +  ω × 10⁻⁶ × T

T_opt = ( σ / (2 × ω × 10⁻⁶ × √rate) ) ^ (2/3)

// -----------------------------------------------------------------------
// pi_controller -- proportional-integral control of a clock's rate
// from a phase error.
//
// The plant is a pure integrator (rate in, phase out), so the closed
// loop is second order:  s^2 + Kp*s + Ki = 0, giving
//   wn   = sqrt(Ki)          natural frequency
//   zeta = Kp / (2*sqrt(Ki)) damping
// Section 6 picks them.
// -----------------------------------------------------------------------
module pi_controller
  import servo_pkg::*;
#(
  // Gains in Q16 fixed point. Section 6's f_c = 3 Hz, zeta = 0.707:
  //   wn = 18.85 rad/s, Kp = 26.66, Ki = 355.3  -- scaled per sample.
  parameter int KP_Q16 = 32'sd1747,      // 0.02666 in Q16
  parameter int KI_Q16 = 32'sd23,        // 0.000355 in Q16
  parameter int INT_LIMIT_PPB = 200_000  // +/- 200 ppm of authority
)(
  input  logic  clk,
  input  logic  rst_n,

  input  logic  err_valid,
  input  off_t  err_ns,
  input  logic  freeze_integral,      // holdover: stop integrating
  input  logic  reset_loop,

  output logic  trim_valid,
  output ppb_t  trim_ppb,
  output ppb_t  integral_ppb,         // the measured drift -- section 16
  output logic  integral_saturated,
  output logic [31:0] c_updates,
  output logic [31:0] c_saturations
);

  logic signed [63:0] integ;
  logic signed [63:0] prop;
  logic signed [63:0] sum;

  always_ff @(posedge clk or negedge rst_n) begin
    if (!rst_n || reset_loop) begin
      integ <= '0; trim_valid <= 1'b0; trim_ppb <= '0;
      integral_ppb <= '0; integral_saturated <= 1'b0;
      c_updates <= '0; c_saturations <= '0;
    end else begin
      trim_valid         <= 1'b0;
      integral_saturated <= 1'b0;

      if (err_valid) begin
        // The clock is AHEAD by err_ns, so it must be SLOWED: the
        // correction is negative. Getting this sign wrong produces a
        // loop that diverges rather than one that converges, which is
        // at least unmistakable.
        prop = -(64'sd0 + 64'(err_ns)) * 64'sd1_000_000 * KP_Q16 >>> 16;

        if (!freeze_integral) begin
          integ <= integ - ((64'(err_ns) * 64'sd1_000_000 * KI_Q16) >>> 16);
        end

        sum = prop + integ;

        // Integral windup must be bounded, and the bound is PHYSICAL:
        // no oscillator this design supports is more than 200 ppm off,
        // so a larger integral means the loop is chasing something
        // that is not a frequency error.
        if (integ > 64'sd(INT_LIMIT_PPB)) begin
          integ              <= 64'sd(INT_LIMIT_PPB);
          integral_saturated <= 1'b1;
          c_saturations      <= c_saturations + 1;
        end else if (integ < -64'sd(INT_LIMIT_PPB)) begin
          integ              <= -64'sd(INT_LIMIT_PPB);
          integral_saturated <= 1'b1;
          c_saturations      <= c_saturations + 1;
        end

        trim_valid   <= 1'b1;
        trim_ppb     <= ppb_t'(sum);
        integral_ppb <= ppb_t'(integ);
        c_updates    <= c_updates + 1;
      end
    end
  end

endmodule

L(s) = (Kp + Ki/s) / s

s² + Kp·s + Ki = 0

ωn   = √Ki            the natural frequency
ζ    = Kp / (2√Ki)    the damping ratio

// -----------------------------------------------------------------------
// rate_trim_applier -- converts a ppb correction into 16.1 section 3's
// fixed-point increment, with a rate limit.
//
// The limit is not about the loop. It is about every consumer that
// measures an INTERVAL using this clock: a trim of X ppb makes every
// measured duration wrong by X parts per billion for as long as it is
// applied. Section 8.
// -----------------------------------------------------------------------
module rate_trim_applier
  import servo_pkg::*;
#(
  parameter int CLK_MHZ    = 156,
  parameter int INC_FRAC_W = 16,
  // What consumers tolerate as a frequency error. 1 ppm is generous
  // for most; a test instrument may demand far less.
  parameter int MAX_TRIM_PPB = 1000,
  // How fast the trim may change, so a step in the loop's output does
  // not become a step in the clock's rate.
  parameter int MAX_SLEW_PPB_PER_UPDATE = 50
)(
  input  logic  clk,
  input  logic  rst_n,

  input  logic  trim_valid,
  input  ppb_t  trim_ppb,

  output logic  adj_valid,
  output logic [23:0] increment,        // 8.16 fixed point, 16.1 section 3
  output ppb_t  applied_ppb,
  output logic  trim_clipped,
  output logic  trim_slewed,
  output logic [31:0] c_clipped,
  output logic [31:0] c_slewed
);

  // Nominal increment: 1e9 / (CLK_MHZ * 1e6) ns per tick, in Q16.
  localparam int NOMINAL =
    (1_000_000_000 << INC_FRAC_W) / (CLK_MHZ * 1_000_000);

  ppb_t cur;

  always_ff @(posedge clk or negedge rst_n) begin
    if (!rst_n) begin
      cur <= '0; adj_valid <= 1'b0; increment <= 24'(NOMINAL);
      applied_ppb <= '0; trim_clipped <= 1'b0; trim_slewed <= 1'b0;
      c_clipped <= '0; c_slewed <= '0;
    end else begin
      adj_valid    <= 1'b0;
      trim_clipped <= 1'b0;
      trim_slewed  <= 1'b0;

      if (trim_valid) begin
        automatic ppb_t want, next;
        // Clip to what consumers tolerate.
        want = trim_ppb;
        if (want > ppb_t'(MAX_TRIM_PPB)) begin
          want         = ppb_t'(MAX_TRIM_PPB);
          trim_clipped <= 1'b1;
          c_clipped    <= c_clipped + 1;
        end else if (want < -ppb_t'(MAX_TRIM_PPB)) begin
          want         = -ppb_t'(MAX_TRIM_PPB);
          trim_clipped <= 1'b1;
          c_clipped    <= c_clipped + 1;
        end

        // Slew-limit the change, so a loop transient does not become a
        // rate discontinuity that downstream interval measurements see.
        next = want;
        if ((want - cur) > ppb_t'(MAX_SLEW_PPB_PER_UPDATE)) begin
          next        = cur + ppb_t'(MAX_SLEW_PPB_PER_UPDATE);
          trim_slewed <= 1'b1;
          c_slewed    <= c_slewed + 1;
        end else if ((cur - want) > ppb_t'(MAX_SLEW_PPB_PER_UPDATE)) begin
          next        = cur - ppb_t'(MAX_SLEW_PPB_PER_UPDATE);
          trim_slewed <= 1'b1;
          c_slewed    <= c_slewed + 1;
        end

        cur         <= next;
        applied_ppb <= next;
        // increment = NOMINAL * (1 + ppb/1e9), in Q16.
        increment   <= 24'(NOMINAL +
                          ((64'(NOMINAL) * 64'(next)) / 64'sd1_000_000_000));
        adj_valid   <= 1'b1;
      end
    end
  end

endmodule

// -----------------------------------------------------------------------
// servo_lock_detector -- the servo's state machine.
//
// Lock is not "the error is small". An error can be small while the
// loop is diverging through zero, and small for one sample while the
// loop has not responded at all. Lock requires the error to STAY small
// for longer than the loop's own time constant -- section 19's
// rejected property is the version that forgets this.
// -----------------------------------------------------------------------
module servo_lock_detector
  import servo_pkg::*;
#(
  parameter int LOCK_WINDOW_NS   = 1000,          // 1 us
  // Several loop time constants. At f_c = 3 Hz the time constant is
  // 53 ms, so 500 ms is about nine of them.
  parameter int LOCK_HOLD_CYCLES = 250_000_000,   // 500 ms at 500 MHz
  parameter int LOSS_CYCLES      = 5_000_000_000/2 // 5 s at 500 MHz
)(
  input  logic  clk,
  input  logic  rst_n,

  input  logic  err_valid,
  input  off_t  err_ns,
  input  logic  reference_present,
  input  logic  integral_saturated,
  input  logic  have_drift_estimate,

  output servo_state_e state,
  output logic  freeze_integral,
  output logic  locked,
  output logic [31:0] time_in_state_ms,
  output logic [31:0] c_locks,
  output logic [31:0] c_lock_losses
);

  logic [63:0] in_window;
  logic [63:0] no_ref;
  logic [63:0] state_cycles;
  servo_state_e st;

  function automatic off_t absv(input off_t v);
    absv = v[47] ? (~v + 1'b1) : v;
  endfunction

  always_ff @(posedge clk or negedge rst_n) begin
    if (!rst_n) begin
      st <= S_FREERUN; in_window <= '0; no_ref <= '0;
      state_cycles <= '0; freeze_integral <= 1'b1;
      locked <= 1'b0; time_in_state_ms <= '0;
      c_locks <= '0; c_lock_losses <= '0;
    end else begin
      state_cycles     <= state_cycles + 1;
      time_in_state_ms <= state_cycles / 500_000;

      no_ref <= reference_present ? '0 : (no_ref + 1);

      if (err_valid) begin
        // The window counter measures how long the error has STAYED
        // inside the band, which is the property that matters. A
        // single sample inside it proves nothing.
        if (absv(err_ns) <= off_t'(LOCK_WINDOW_NS)) in_window <= in_window + 1;
        else                                        in_window <= '0;
      end

      unique case (st)
        S_FREERUN: begin
          freeze_integral <= 1'b1;
          locked          <= 1'b0;
          if (reference_present) begin
            st <= S_ACQUIRE; state_cycles <= '0; in_window <= '0;
          end
        end

        S_ACQUIRE: begin
          freeze_integral <= 1'b0;
          locked          <= 1'b0;
          if (!reference_present && (no_ref > 64'(LOSS_CYCLES)))
            st <= have_drift_estimate ? S_HOLDOVER : S_FREERUN;
          else if (integral_saturated && (state_cycles > 64'(LOCK_HOLD_CYCLES)))
            // The loop has had time and is still asking for more
            // authority than exists. That is not acquisition.
            st <= S_FAULT;
          else if (in_window > 64'(LOCK_HOLD_CYCLES)) begin
            st           <= S_LOCKED;
            state_cycles <= '0;
            c_locks      <= c_locks + 1;
          end
        end

        S_LOCKED: begin
          freeze_integral <= 1'b0;
          locked          <= 1'b1;
          if (!reference_present && (no_ref > 64'(LOSS_CYCLES))) begin
            st            <= S_HOLDOVER;
            state_cycles  <= '0;
            c_lock_losses <= c_lock_losses + 1;
          end else if (in_window == '0) begin
            st            <= S_ACQUIRE;
            state_cycles  <= '0;
            c_lock_losses <= c_lock_losses + 1;
          end
        end

        S_HOLDOVER: begin
          // Keep applying the last known drift. The integral holds its
          // value and the proportional term has no input -- section 16.
          freeze_integral <= 1'b1;
          locked          <= 1'b0;
          if (reference_present) begin
            st <= S_ACQUIRE; state_cycles <= '0; in_window <= '0;
          end
        end

        S_FAULT: begin
          freeze_integral <= 1'b1;
          locked          <= 1'b0;
          if (!reference_present) st <= S_FREERUN;
        end
      endcase
    end
  end

  assign state = st;

endmodule

// -----------------------------------------------------------------------
// residence_timer -- measures the interval between a PTP event
// message's arrival SFD and its departure SFD.
//
// Both captures come from 16.3 section 3's unit. The value is not
// known until the frame departs -- and the departure SFD precedes
// correctionField by 22 octets, so it exists when the field needs it.
// -----------------------------------------------------------------------
module residence_timer
  import servo_pkg::*;
#(
  parameter int PENDING = 16          // frames in flight through the switch
)(
  input  logic  clk,
  input  logic  rst_n,

  // Ingress: an event message arrived.
  input  logic        in_valid,
  input  logic [15:0] in_tag,
  input  logic [47:0] in_sec,
  input  logic [31:0] in_ns,

  // Egress: the same message is leaving.
  input  logic        out_valid,
  input  logic [15:0] out_tag,
  input  logic [47:0] out_sec,
  input  logic [31:0] out_ns,

  output logic        residence_valid,
  output logic [15:0] residence_tag,
  output correction_t residence_16_16,
  output logic        negative_residence,
  output logic [31:0] c_measured,
  output logic [31:0] c_no_ingress,
  output logic [31:0] c_abandoned,
  output logic [31:0] longest_ns
);

  typedef struct packed {
    logic        busy;
    logic [15:0] tag;
    logic [47:0] sec;
    logic [31:0] ns;
    logic [31:0] age;
  } pend_t;

  pend_t p [PENDING];

  // A frame that entered and never left must free its slot: 14.1's
  // allocator can discard a frame at the egress, and the transparent
  // clock has no other way to learn that it did.
  localparam int MAX_AGE = 32'h0400_0000;   // ~134 ms at 500 MHz

  always_ff @(posedge clk or negedge rst_n) begin
    int i;
    automatic bit placed, found;
    if (!rst_n) begin
      for (i = 0; i < PENDING; i++) p[i] <= '0;
      residence_valid <= 1'b0; residence_tag <= '0;
      residence_16_16 <= '0; negative_residence <= 1'b0;
      c_measured <= '0; c_no_ingress <= '0;
      c_abandoned <= '0; longest_ns <= '0;
    end else begin
      residence_valid    <= 1'b0;
      negative_residence <= 1'b0;

      for (i = 0; i < PENDING; i++) begin
        if (p[i].busy) begin
          p[i].age <= p[i].age + 1;
          if (p[i].age == MAX_AGE) begin
            p[i].busy   <= 1'b0;
            c_abandoned <= c_abandoned + 1;
          end
        end
      end

      if (in_valid) begin
        placed = 1'b0;
        for (i = 0; i < PENDING; i++) begin
          if (!p[i].busy && !placed) begin
            placed    = 1'b1;
            p[i].busy <= 1'b1;
            p[i].tag  <= in_tag;
            p[i].sec  <= in_sec;
            p[i].ns   <= in_ns;
            p[i].age  <= '0;
          end
        end
      end

      if (out_valid) begin
        found = 1'b0;
        for (i = 0; i < PENDING; i++) begin
          if (p[i].busy && (p[i].tag == out_tag)) begin
            automatic logic signed [63:0] d;
            found = 1'b1;
            d = ($signed(64'(out_sec)) - $signed(64'(p[i].sec)))
                  * 64'sd1_000_000_000
              + ($signed(64'(out_ns))  - $signed(64'(p[i].ns)));
            p[i].busy <= 1'b0;

            // A negative residence is physically impossible and is
            // therefore evidence -- a tag collision, or a clock
            // stepped while the frame was inside us.
            if (d < 0) begin
              negative_residence <= 1'b1;
            end else begin
              residence_valid <= 1'b1;
              residence_tag   <= out_tag;
              // Shift into 16.16 fixed point. The fractional bits stay
              // zero here; 16.5 is where sub-nanosecond residence
              // measurement becomes worth having.
              residence_16_16 <= d <<< 16;
              c_measured      <= c_measured + 1;
              if (d[31:0] > longest_ns) longest_ns <= d[31:0];
            end
          end
        end
        if (!found) c_no_ingress <= c_no_ingress + 1;
      end
    end
  end

endmodule

// -----------------------------------------------------------------------
// correction_accumulator -- decides WHAT a transparent clock adds to
// correctionField, and on which messages.
//
// 16.3 section 10 built the in-flight rewrite. This module supplies
// its add_value and its enable, and the two decisions here are the
// ones that make a path of transparent clocks compose correctly.
// -----------------------------------------------------------------------
module correction_accumulator
  import servo_pkg::*;
(
  input  logic        clk,
  input  logic        rst_n,

  input  logic        residence_valid,
  input  logic [15:0] residence_tag,
  input  correction_t residence_16_16,

  // What kind of message is leaving, and on which port.
  input  logic        msg_is_event,
  input  logic        msg_is_pdelay,      // peer-delay messages are special
  input  logic [15:0] egress_tag,

  // The link's own asymmetry correction, if one has been calibrated.
  // 16.5 section 11 produces it; this module applies it.
  input  correction_t cfg_egress_asymmetry,
  input  logic        cfg_asymmetry_valid,

  output logic        add_valid,
  output correction_t add_value,
  output logic        add_enable,
  output logic [31:0] c_applied,
  output logic [31:0] c_suppressed_general,
  output logic [31:0] c_asymmetry_applied
);

  always_ff @(posedge clk or negedge rst_n) begin
    if (!rst_n) begin
      add_valid <= 1'b0; add_value <= '0; add_enable <= 1'b0;
      c_applied <= '0; c_suppressed_general <= '0;
      c_asymmetry_applied <= '0;
    end else begin
      add_valid  <= 1'b0;
      add_enable <= 1'b0;

      if (residence_valid && (residence_tag == egress_tag)) begin
        // Decision 1: EVENT messages only. A Follow_Up's correction
        // already describes the Sync it accompanies; adding this
        // switch's residence time to it corrupts a correct value --
        // 16.3 section 10's production note, as a policy.
        if (!msg_is_event) begin
          c_suppressed_general <= c_suppressed_general + 1;
        end else begin
          automatic correction_t v;
          v = residence_16_16;

          // Decision 2: a calibrated link asymmetry is folded in here,
          // because correctionField is the only place in the protocol
          // where a per-hop constant can be carried.
          if (cfg_asymmetry_valid) begin
            v = v + cfg_egress_asymmetry;
            c_asymmetry_applied <= c_asymmetry_applied + 1;
          end

          add_valid  <= 1'b1;
          add_value  <= v;
          add_enable <= 1'b1;
          c_applied  <= c_applied + 1;
        end
      end
    end
  end

endmodule

// -----------------------------------------------------------------------
// servo_telemetry -- reports what the loop is achieving alongside what
// its inputs make achievable, so a servo doing its best on a bad path
// is distinguishable from one that is mistuned.
// -----------------------------------------------------------------------
module servo_telemetry
  import servo_pkg::*;
#(
  parameter int SAMPLE_RATE_HZ = 16
)(
  input  logic  clk,
  input  logic  rst_n,

  input  logic  err_valid,
  input  off_t  err_ns,
  input  off_t  spread_ns,           // section 3's measurement noise
  input  ppb_t  integral_ppb,
  input  servo_state_e state,
  input  logic [31:0] correction_interval_ms,
  input  logic  window_tick,

  output off_t  achievable_ns,       // section 4's total(T)
  output off_t  achieved_ns,
  output logic  performing_to_inputs,
  output logic  bandwidth_is_sane,
  output ppb_t  measured_drift_ppb,
  output logic [31:0] holdover_ns_per_s,
  output logic [31:0] c_windows
);

  off_t worst;

  function automatic off_t absv(input off_t v);
    absv = v[47] ? (~v + 1'b1) : v;
  endfunction

  always_ff @(posedge clk or negedge rst_n) begin
    if (!rst_n) begin
      worst <= '0; achievable_ns <= '0; achieved_ns <= '0;
      performing_to_inputs <= 1'b0; bandwidth_is_sane <= 1'b0;
      measured_drift_ppb <= '0; holdover_ns_per_s <= '0;
      c_windows <= '0;
    end else begin
      if (err_valid && (absv(err_ns) > worst)) worst <= absv(err_ns);

      if (window_tick) begin
        automatic off_t noise_term, drift_term;
        // Section 4's two terms, evaluated at the interval in use.
        // sqrt is approximated by a shift-based estimate in hardware;
        // the shape is what matters, not the third digit.
        noise_term = spread_ns / off_t'(isqrt(SAMPLE_RATE_HZ *
                                  (correction_interval_ms / 1000 + 1)));
        drift_term = (off_t'(measured_drift_ppb) *
                      off_t'(correction_interval_ms)) / 1_000_000;

        achievable_ns <= noise_term + drift_term;
        achieved_ns   <= worst;
        worst         <= '0;

        // Within 2x of the floor is as good as it gets.
        performing_to_inputs <= (worst <= (2 * (noise_term + drift_term)));

        // Section 6's two outside constraints, as a single bit.
        bandwidth_is_sane <= (correction_interval_ms >=
                              (32'd10_000 / SAMPLE_RATE_HZ));

        // The integrator IS the drift estimate -- section 5.
        measured_drift_ppb <= integral_ppb;
        holdover_ns_per_s  <= (integral_ppb < 0) ? 32'(-integral_ppb)
                                                 : 32'(integral_ppb);
        c_windows <= c_windows + 1;
      end
    end
  end

endmodule

// -----------------------------------------------------------------------
// servo_conformance_monitor -- one bit.
//
// It asserts that the servo behaved as a servo must: rate trims only,
// bounded authority, a dwell before declaring lock, and a transparent
// clock that adds to the right messages. It does NOT assert that the
// clock is correct -- 16.2 section 8's bias is still there.
// -----------------------------------------------------------------------
module servo_conformance_monitor
  import servo_pkg::*;
(
  input  logic  clk,
  input  logic  rst_n,

  input  logic  stepped_after_first,      // 16.1 section 3's ADJ_OFFSET
  input  logic  locked_without_dwell,
  input  logic  integral_wound_unbounded,
  input  logic  trim_exceeded_consumer_limit,
  input  logic  corrected_a_general_message,
  input  logic  negative_residence,
  input  logic  residence_added_twice,
  input  logic  cfg_bandwidth_above_rate,  // section 6's first constraint
  input  logic  cfg_filter_delay_excessive,// section 6's second
  input  logic  cfg_lock_window_unreachable,

  output logic  conformant,
  output logic [15:0] fault_vector,
  output logic [31:0] c_violations
);

  logic v_step, v_lock, v_wind, v_trim, v_gen, v_neg, v_twice;
  logic v_bw, v_filt, v_win;

  always_ff @(posedge clk or negedge rst_n) begin
    if (!rst_n) begin
      v_step <= 1'b0; v_lock <= 1'b0; v_wind <= 1'b0; v_trim <= 1'b0;
      v_gen <= 1'b0; v_neg <= 1'b0; v_twice <= 1'b0;
      v_bw <= 1'b0; v_filt <= 1'b0; v_win <= 1'b0;
      c_violations <= '0;
    end else begin
      if (stepped_after_first)          begin v_step  <= 1'b1; c_violations <= c_violations + 1; end
      if (locked_without_dwell)         begin v_lock  <= 1'b1; c_violations <= c_violations + 1; end
      if (integral_wound_unbounded)     begin v_wind  <= 1'b1; c_violations <= c_violations + 1; end
      if (trim_exceeded_consumer_limit) begin v_trim  <= 1'b1; c_violations <= c_violations + 1; end
      if (corrected_a_general_message)  begin v_gen   <= 1'b1; c_violations <= c_violations + 1; end
      if (negative_residence)           begin v_neg   <= 1'b1; c_violations <= c_violations + 1; end
      if (residence_added_twice)        begin v_twice <= 1'b1; c_violations <= c_violations + 1; end

      // Standing configuration properties -- section 6's two outside
      // constraints and a lock window the floor cannot reach.
      v_bw   <= cfg_bandwidth_above_rate;
      v_filt <= cfg_filter_delay_excessive;
      v_win  <= cfg_lock_window_unreachable;
    end
  end

  assign conformant = !(v_step || v_lock || v_wind || v_trim || v_gen ||
                        v_neg || v_twice || v_bw || v_filt || v_win);
  assign fault_vector = {6'b0, v_win, v_filt, v_bw, v_twice, v_neg,
                         v_gen, v_trim, v_wind, v_lock, v_step};

endmodule

// P1. An untrustworthy sample never enters the window. It is not an
// outlier, it is not a measurement -- 16.2 section 15.
property p_untrusted_rejected;
  @(posedge clk) disable iff (!rst_n)
  (sample_valid && !sample_trustworthy) |=> $stable(win);
endproperty

// P2. A grossly wrong sample never enters either.
property p_gross_rejected;
  @(posedge clk) disable iff (!rst_n)
  (sample_valid && (offset_ns > off_t'(REJECT_NS))) |=> $stable(win);
endproperty

// P3. The output is a member of the window -- a median always is,
// which a mean is not. This catches an arithmetic slip directly.
property p_median_is_a_sample;
  @(posedge clk) disable iff (!rst_n)
  err_valid |-> (err_ns inside {win[0], win[1], win[2], win[3], win[4]});
endproperty

// P4. Up to two outliers of any magnitude do not move the output.
property p_median_rejects_two;
  @(posedge clk) disable iff (!rst_n)
  (two_samples_replaced_by_outliers) |=> $stable(err_ns);
endproperty

// P5. Nothing is published until the window is full.
property p_needs_full_window;
  @(posedge clk) disable iff (!rst_n)
  err_valid |-> (fill == N);
endproperty

// P6. The correction opposes the error. Getting this backwards makes
// the loop diverge, which is at least unmistakable.
property p_correction_opposes_error;
  @(posedge clk) disable iff (!rst_n)
  (err_valid && (err_ns > 0)) |=> (trim_ppb < $past(trim_ppb));
endproperty

// P7. The integral is bounded by PHYSICS -- no supported oscillator
// is more than 200 ppm off.
property p_integral_is_bounded;
  @(posedge clk) disable iff (!rst_n)
  (integ <= 64'sd(INT_LIMIT_PPB)) && (integ >= -64'sd(INT_LIMIT_PPB));
endproperty

// P8. Saturation is reported, not silently clamped.
property p_saturation_reported;
  @(posedge clk) disable iff (!rst_n)
  (integ == 64'sd(INT_LIMIT_PPB)) |-> integral_saturated;
endproperty

// P9. A frozen integral does not move -- holdover depends on it.
property p_freeze_holds;
  @(posedge clk) disable iff (!rst_n)
  (err_valid && freeze_integral) |=> $stable(integ);
endproperty

// P10. With zero error the proportional term is zero and only the
// integral remains -- which is the steady state.
property p_steady_state_is_integral;
  @(posedge clk) disable iff (!rst_n)
  (err_valid && (err_ns == 0)) |=> (trim_ppb == ppb_t'(integ));
endproperty

// P11. The integral IS the drift estimate that section 16 needs.
property p_integral_is_the_drift;
  @(posedge clk) disable iff (!rst_n)
  trim_valid |-> (integral_ppb == ppb_t'(integ));
endproperty

// P12. Never a step. 16.1 section 3's ADJ_OFFSET is not used after
// the initial time-of-day set.
property p_never_steps;
  @(posedge clk) disable iff (!rst_n)
  (c_steps > 32'd1) |-> 1'b0;
endproperty

// P13. The applied trim never exceeds what consumers tolerate.
property p_trim_within_consumer_limit;
  @(posedge clk) disable iff (!rst_n)
  adj_valid |-> ((applied_ppb <= ppb_t'(MAX_TRIM_PPB)) &&
                 (applied_ppb >= -ppb_t'(MAX_TRIM_PPB)));
endproperty

// P14. And never changes faster than the slew limit, so a loop
// transient does not become a rate discontinuity.
property p_slew_limited;
  @(posedge clk) disable iff (!rst_n)
  adj_valid |-> (absv(applied_ppb - $past(applied_ppb)) <=
                 ppb_t'(MAX_SLEW_PPB_PER_UPDATE));
endproperty

// P15. A zero trim produces exactly the nominal increment.
property p_zero_trim_is_nominal;
  @(posedge clk) disable iff (!rst_n)
  (adj_valid && (applied_ppb == 0)) |-> (increment == 24'(NOMINAL));
endproperty

// P16. Clipping is reported.
property p_clip_reported;
  @(posedge clk) disable iff (!rst_n)
  (trim_valid && (trim_ppb > ppb_t'(MAX_TRIM_PPB))) |=> trim_clipped;
endproperty

// P17. Lock requires the error to have STAYED in the window for
// longer than the loop's own time constant. Section 19's rejected
// property is the version that omits the dwell.
property p_lock_requires_dwell;
  @(posedge clk) disable iff (!rst_n)
  $rose(locked) |-> ($past(in_window) > 64'(LOCK_HOLD_CYCLES));
endproperty

// P18. Losing the reference goes to HOLDOVER if a drift estimate
// exists, and to FREERUN if not. The two are different states.
property p_holdover_needs_an_estimate;
  @(posedge clk) disable iff (!rst_n)
  ((state == S_HOLDOVER) && $past(state != S_HOLDOVER)) |-> $past(have_drift_estimate);
endproperty

// P19. HOLDOVER freezes the integral -- that is what holdover IS.
property p_holdover_freezes;
  @(posedge clk) disable iff (!rst_n)
  (state == S_HOLDOVER) |-> freeze_integral;
endproperty

// P20. A saturated integral after a full dwell is a FAULT, not
// acquisition. Leaving it in ACQUIRE reports "converging" for ever.
property p_persistent_saturation_is_a_fault;
  @(posedge clk) disable iff (!rst_n)
  ((state == S_ACQUIRE) && integral_saturated &&
   (state_cycles > 64'(LOCK_HOLD_CYCLES))) |=> (state == S_FAULT);
endproperty

// P21. locked is true only in S_LOCKED.
property p_locked_iff_state;
  @(posedge clk) disable iff (!rst_n)
  locked <-> (state == S_LOCKED);
endproperty

// P22. A residence time is non-negative. It is a physical duration
// and a negative value is evidence about the inputs.
property p_residence_is_physical;
  @(posedge clk) disable iff (!rst_n)
  residence_valid |-> (residence_16_16 >= 0);
endproperty

// P23. A residence measurement pairs an egress to the ingress with
// the SAME tag -- never by order.
property p_residence_pairs_by_tag;
  @(posedge clk) disable iff (!rst_n)
  residence_valid |-> (residence_tag == $past(out_tag));
endproperty

// P24. Every tracked frame eventually leaves the tracker -- measured
// or aged out. 14.1's allocator can discard one at the egress.
property p_no_permanent_pending;
  @(posedge clk) disable iff (!rst_n)
  $rose(p[i].busy) |-> ##[1:MAX_AGE+1] !p[i].busy;
endproperty

// P25. The correction is added to EVENT messages only.
property p_correction_events_only;
  @(posedge clk) disable iff (!rst_n)
  add_valid |-> msg_is_event;
endproperty

// P26. And added exactly once per hop.
property p_added_once_per_hop;
  @(posedge clk) disable iff (!rst_n)
  add_valid |=> !add_valid until_with next_frame;
endproperty

// P27. The correction accumulates: the emitted value is the received
// value plus this switch's contribution.
property p_correction_accumulates;
  @(posedge clk) disable iff (!rst_n)
  add_valid |-> (emitted_correction == (received_correction + add_value));
endproperty

// P28. Section 6's first outside constraint: the loop bandwidth is
// well below the sample rate.
property p_bandwidth_below_sample_rate;
  @(posedge clk) disable iff (!rst_n)
  !cfg_bandwidth_above_rate;
endproperty

// P29. And the second: the filter's group delay is small against the
// loop's time constant.
property p_filter_delay_is_small;
  @(posedge clk) disable iff (!rst_n)
  !cfg_filter_delay_excessive;
endproperty

// P30. The lock window is reachable -- wider than section 4's floor.
// Otherwise the servo converges correctly and never reports lock.
property p_lock_window_is_reachable;
  @(posedge clk) disable iff (!rst_n)
  (off_t'(LOCK_WINDOW_NS) > achievable_ns);
endproperty

// P31. conformant is exactly the conjunction it claims.
property p_conformant_definition;
  @(posedge clk) disable iff (!rst_n)
  conformant <-> (fault_vector == 16'h0000);
endproperty