#include "lip/app/ptp/lip_ptp_cfg_defs.h"
#if LIP_PTP_ENABLE
#include "lip_ptp_md_eth_sync_send.h"
#include "lip/app/ptp/misc/lip_ptp_tstmp_ops.h"

void lip_ptp_md_eth_sync_send_init(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_eth_sync_send_ctx *ctx = &p->md.eth.sync_send;
    memset(ctx, 0, sizeof(t_lip_ptp_eth_sync_send_ctx));
}


void lip_ptp_md_eth_send_sync(t_lip_ptp_port_ctx *p, const t_lip_ptp_port_md_sync_info *info) {
    const int msg_size = sizeof(t_lip_ptp_msg_sync);
    t_lip_ptp_eth_sync_send_ctx *ctx = &p->md.eth.sync_send;
    /* 11.2.15.2.1 setSyncTwoStep() */
    t_lip_tx_frame *frame = lip_ptp_prepare_frame(
        p, LIP_PTP_MSGTYPE_SYNC, msg_size, 0);
    if (frame) {
        t_lip_ptp_msg_sync *ptp_msg = (t_lip_ptp_msg_sync *)frame->buf.cur_ptr;
        ptp_msg->hdr.corField = 0;  /*  correctionField is set equal to 0. */
        /* b) sourcePortIdentity заполняется по умолчанию от порта (не видно смысла передавать через info) */
        ++ctx->tx_sync_seqid;
        ptp_msg->hdr.seqId = HTON16(ctx->tx_sync_seqid); /* c) sequenceId */
        ptp_msg->hdr.logMsgInterval = info->logMessageInterval; /* d) logMessageInterval from MDSyncSend */
        ptp_msg->hdr.flags0 = LIP_PTP_MSGFLAG0_TWO_STEP;
        lip_ptp_frame_flush(frame, msg_size);
        ctx->tx_wt_fup = true;
        memcpy(&ctx->tx_info, info, sizeof(*info));
    }
}

void lip_ptp_md_eth_send_sync_etstmp(t_lip_ptp_port_ctx *p, const t_lip_ptp_tstmp *etstmp) {
    t_lip_ptp_eth_sync_send_ctx *ctx = &p->md.eth.sync_send;
    if (ctx->tx_wt_fup) {
        const int msg_size = sizeof(t_lip_ptp_msg_follow_up) + LIP_PTP_AS_PTP_TLV_TOTAL_SIZE;
        t_lip_tx_frame *frame = lip_ptp_prepare_frame(
            p, LIP_PTP_MSGTYPE_FOLLOW_UP, msg_size, 0);
        if (frame) {
            t_lip_ptp_msg_follow_up *ptp_msg = (t_lip_ptp_msg_follow_up *)frame->buf.cur_ptr;
            /* 11.2.15.2.3 setFollowUp() */
            /* a) followUpCorrectionField = MDSyncSend.followUpCorrectionField + rateRatio * (syncEventEgressTimestamp - upstreamTxTime) */

            t_lip_fns cor = lip_ptp_tstmp_delta_fns(etstmp, &ctx->tx_info.upstreamTxTime);
            cor = ctx->tx_info.fupCorrection + ctx->tx_info.rateRatio * cor;
            ptp_msg->hdr.corField = HTON64(cor);
            /* b) sourcePortIdentity заполняется автоматом по порту */
            ptp_msg->hdr.seqId = HTON16(ctx->tx_sync_seqid); /* c) sequenceId = syn.sequenceId */
            ptp_msg->hdr.logMsgInterval = ctx->tx_info.logMessageInterval; /* d) logMessageInterval from MDSyncSend */
            memcpy(&ptp_msg->precOriginTstmp, &ctx->tx_info.preciseOriginTimestamp, LIP_PTP_PKT_TSTMP_SIZE);
            uint8_t *opt_ptr = &frame->buf.cur_ptr[sizeof(t_lip_ptp_msg_signaling)];
            const uint16_t opt_len = LIP_PTP_TLV_ORG_HDR_SIZE + sizeof(t_lip_ptp_orgtlv_gptp_fup);
            opt_ptr = f_set_tlv_hdr(opt_ptr, LIP_PTP_TLV_CODE_ORG_EXT, opt_len);
            opt_ptr = f_set_tlv_org_id(opt_ptr, LIP_PTP_ORGID_CODE_GPTP, LIP_PTP_ORG_GPTP_SUBTYPE_FUP);
            t_lip_ptp_orgtlv_gptp_fup *fup_tlv = (t_lip_ptp_orgtlv_gptp_fup *)opt_ptr;
            int32_t rateOffs = (int32_t)((ctx->tx_info.rateRatio - 1) * (1ULL << 41)); /* see 11.4.4.3.6 IEEE 802.1AS-2020  */
            fup_tlv->cumulativeScaledRateOffset = HTON32(rateOffs);
            memcpy(&fup_tlv->lastChange, &ctx->tx_info.lastChange, sizeof(t_lip_ptp_last_time_change_info));
            lip_ptp_frame_flush(frame, msg_size);
            ctx->tx_wt_fup = false;
        }
    }

}

#endif
