87 lines
3.5 KiB
C++
87 lines
3.5 KiB
C++
// SPDX-FileCopyrightText: 2024 Filip Leonarski, Paul Scherrer Institute <filip.leonarski@psi.ch>
|
|
// SPDX-License-Identifier: CERN-OHL-S-2.0
|
|
|
|
#include "hls_jfjoch.h"
|
|
|
|
void ptp(AXI_STREAM ð_in,
|
|
volatile ap_uint<80> &counter_ns,
|
|
volatile ap_uint<32> &counter_error,
|
|
volatile ap_uint<1> &synced_to_ptp) {
|
|
#pragma HLS INTERFACE ap_ctrl_none port=return
|
|
#pragma HLS INTERFACE axis register both port=eth_in
|
|
#pragma HLS INTERFACE ap_none register port=counter_ns
|
|
#pragma HLS INTERFACE ap_none register port=counter_error
|
|
#pragma HLS INTERFACE ap_none register port=synced_to_ptp
|
|
#pragma HLS PIPELINE II=1
|
|
|
|
static bool internal_sync = false;
|
|
static bool first_axi_beat = true;
|
|
static ap_uint<32> internal_counter_ns = 0;
|
|
static ap_uint<48> internal_counter_sec = 0;
|
|
static ap_uint<30> last_sync_5ns = 0;
|
|
static ap_uint<32> internal_counter_error = UINT32_MAX;
|
|
|
|
const ap_uint<8> PTP_SYNC_TYPE = 0x0; // Sync message
|
|
const ap_uint<8> PTP_FOLLOW_UP_TYPE = 0x8; // Follow_up message
|
|
|
|
packet_512_t packet_in;
|
|
if (eth_in.read_nb(packet_in)) {
|
|
if (first_axi_beat) {
|
|
ap_uint<4> ptp_msg_type = packet_in.data.range(14 * 8 + 3, 14 * 8 + 0);
|
|
ap_uint<16> ptp_msg_flags = get_header_field_16(packet_in.data, 14 * 8 + 48);
|
|
ap_uint<1> ptp_msg_two_step = ptp_msg_flags[1];
|
|
|
|
if ((!ptp_msg_two_step && ptp_msg_type == PTP_SYNC_TYPE) || ptp_msg_type == PTP_FOLLOW_UP_TYPE) {
|
|
// Extract new PTP time from packet
|
|
ap_uint<32> ptp_ns = get_header_field_32(packet_in.data, (14 + 34) * 8);
|
|
ap_uint<48> ptp_sec = get_header_field_48(packet_in.data, (14 + 34) * 8 + 32);
|
|
|
|
// Compute absolute time difference in ns between current and PTP-provided time
|
|
ap_uint<80> curr_total_ns = (ap_uint<80>)internal_counter_sec * (ap_uint<80>)1000000000ULL
|
|
+ (ap_uint<80>)internal_counter_ns;
|
|
ap_uint<80> ptp_total_ns = (ap_uint<80>)ptp_sec * (ap_uint<80>)1000000000ULL
|
|
+ (ap_uint<80>)ptp_ns;
|
|
ap_uint<80> diff_ns = (curr_total_ns >= ptp_total_ns)
|
|
? (curr_total_ns - ptp_total_ns)
|
|
: (ptp_total_ns - curr_total_ns);
|
|
|
|
// If more than 1UINT32_MAX; otherwise set exact diff in ns
|
|
if (diff_ns > UINT32_MAX)
|
|
internal_counter_error = UINT32_MAX;
|
|
else
|
|
internal_counter_error = (ap_uint<32>)diff_ns;
|
|
|
|
// Update internal counters to PTP time
|
|
internal_counter_ns = ptp_ns;
|
|
internal_counter_sec = ptp_sec;
|
|
last_sync_5ns = 0;
|
|
internal_sync = true;
|
|
}
|
|
}
|
|
|
|
if (packet_in.last)
|
|
first_axi_beat = true;
|
|
else
|
|
first_axi_beat = false;
|
|
}
|
|
|
|
// more than two seconds synced_to_ptp is false
|
|
if (last_sync_5ns > (2 * 1000 * 1000 * 1000) / 5) // log2() = 28.5
|
|
internal_sync = false;
|
|
|
|
if (internal_sync)
|
|
last_sync_5ns += 1;
|
|
|
|
counter_ns.write((internal_counter_sec, internal_counter_ns));
|
|
synced_to_ptp.write(internal_sync);
|
|
counter_error = internal_counter_error;
|
|
|
|
internal_counter_ns += 5; // 200 MHz == 5 ns
|
|
|
|
// Carry check: If nanoseconds overflows 1 second (1,000,000,000 ns)
|
|
if (internal_counter_ns >= 1000000000UL) {
|
|
internal_counter_sec += 1;
|
|
internal_counter_ns -= 1000000000UL;
|
|
}
|
|
}
|