// 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 &eth_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;
    }
}