#include "lip/app/ptp/lip_ptp_cfg_defs.h"
#if LIP_PTP_ENABLE
#include "lip_ptp_pdelay_resp.h"
#include "lip/app/ptp/lip_ptp_proto_defs.h"
#include "lip/app/ptp/misc/lip_ptp_tstmp_ops.h"
#include <stdbool.h>
#include <string.h>

void lip_ptp_pdelay_resp_init(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_pdelay_resp_ctx *ctx = &p->md.eth.pdelay_resp;
    ctx->state = LIP_PTP_PDELAY_RESP_STATE_INIT;
}


void lip_ptp_pdelay_resp_etstmp(t_lip_ptp_port_ctx *p, const t_lip_ptp_tstmp *etstmp) {
    t_lip_ptp_pdelay_resp_ctx *ctx = &p->md.eth.pdelay_resp;
    if (ctx->state == LIP_PTP_PDELAY_RESP_STATE_WT_RESP_TSTMP) {
        t_lip_tx_frame *tx_frame = lip_ptp_prepare_frame(p, LIP_PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP,
                                                         sizeof(t_lip_ptp_msg_pdelay_resp_follow_up),
                                                         lip_ptp_port_cmlds_mode_is_en(p) ? LIP_PTP_MSG_PREP_FLAG_CMLDS : 0);
        if (tx_frame) {
            /* заполняем поля в соответствии с 11.2.20.3.3 из IEEE 802.1AS-2020 */
            t_lip_ptp_msg_pdelay_resp_follow_up *resp = (t_lip_ptp_msg_pdelay_resp_follow_up*)tx_frame->buf.cur_ptr;
            resp->hdr.seqId = ctx->req_seq_id_be;
            f_host_tstmp_to_pkt(etstmp, &resp->respOriginTstmp);
            if (etstmp->fract_ns != 0) {
                uint64_t corVal = etstmp->fract_ns;
                if (lip_ptp_port_cmlds_mode_is_en(p)) {
                    corVal += NTOH64(ctx->req_cor_be);
                }
                resp->hdr.corField = HTON64(corVal);
            } else {
                resp->hdr.corField = lip_ptp_port_cmlds_mode_is_en(p) ? ctx->req_cor_be : 0;
            }
            memcpy(&resp->reqPortIdentity, &ctx->req_src_id, sizeof(t_lip_ptp_port_identity));
            resp->hdr.logMsgInterval = LIP_PTP_LOGINT_INVALID;
            lip_ptp_frame_flush(tx_frame, sizeof(t_lip_ptp_msg_pdelay_resp_follow_up));
            ctx->state = LIP_PTP_PDELAY_RESP_STATE_WT_REQ;
        }
    }
}



void lip_ptp_pdelay_resp_proc_req(t_lip_ptp_port_ctx *p, t_lip_payload *pl, const t_lip_ptp_rx_frame *frame) {
    if ((pl->size >= sizeof(t_lip_ptp_msg_pdelay_req)) && frame->tstmp_valid) {
        t_lip_ptp_pdelay_resp_ctx *ctx = &p->md.eth.pdelay_resp;
        const t_lip_ptp_msg_pdelay_req *req = (const t_lip_ptp_msg_pdelay_req *)pl->data;
        const bool cmlds_en = frame->sdo_id_mj == LIP_PTP_SDOID_MJ_CMLDS;
        if (cmlds_en) {
            lip_ptp_port_cmlds_mode_set(p);
        } else {
            lip_ptp_port_cmlds_mode_clr(p);
        }

        t_lip_tx_frame *tx_frame = lip_ptp_prepare_frame(p, LIP_PTP_MSGTYPE_PDELAY_RESP, sizeof(t_lip_ptp_msg_pdelay_resp),
                                                         cmlds_en ? LIP_PTP_MSG_PREP_FLAG_CMLDS : 0);
        if (tx_frame) {

            t_lip_ptp_msg_pdelay_resp *resp = (t_lip_ptp_msg_pdelay_resp*)tx_frame->buf.cur_ptr;
            const t_lip_ptp_tstmp *tstmp = &frame->tstmp;

            /* заполняем поля в соответствии с 11.2.20.3.1 из IEEE 802.1AS-2020 */
            resp->hdr.seqId = req->hdr.seqId;
            resp->hdr.flags0 = LIP_PTP_MSGFLAG0_TWO_STEP;
            f_host_tstmp_to_pkt(tstmp, &resp->reqReceiptTstmp);
            if (frame->tstmp.fract_ns != 0) {
                const uint64_t corVal = cmlds_en ? -frame->tstmp.fract_ns : frame->tstmp.fract_ns;
                resp->hdr.corField = HTON64(corVal);
            } else {
                resp->hdr.corField = 0;
            }
            memcpy(&resp->reqPortIdentity, &req->hdr.srcPortIdentity, sizeof(t_lip_ptp_port_identity));
            resp->hdr.logMsgInterval = LIP_PTP_LOGINT_INVALID;
            lip_ptp_frame_flush(tx_frame, sizeof(t_lip_ptp_msg_pdelay_resp));

            memcpy(&ctx->req_src_id, &req->hdr.srcPortIdentity, sizeof(t_lip_ptp_port_identity));
            ctx->req_seq_id_be = req->hdr.seqId;
            ctx->req_cor_be = req->hdr.corField;
            ctx->state = LIP_PTP_PDELAY_RESP_STATE_WT_RESP_TSTMP;
        }
    }
}
#endif

