Commit c9855d2a authored by Cesar Prados's avatar Cesar Prados Committed by Alessandro Rubini

pdelay/pclock: add handler for peer resp msg

Signed-off-by: Cesar Prados's avatarC.Prados <c.prados@gsi.de>
parent 19a746cb
......@@ -8,6 +8,7 @@
#include <ppsi/ppsi.h>
#include "common-fun.h"
#include "../lib/network_types.h"
#include "../proto-ext-whiterabbit/wr-api.h" /* FIXME: phase_to_cf_units */
#ifdef CONFIG_ARCH_WRS
#define ARCH_IS_WRS 1
......@@ -192,6 +193,53 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
return 0;
}
int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
int len)
{
MsgPDelayResp resp;
MsgHeader *hdr = &ppi->received_ptp_header;
if (len < PP_PDELAY_RESP_LENGTH)
return -1;
msg_unpack_pdelay_resp(buf, &resp);
if ((memcmp(&DSPOR(ppi)->portIdentity.clockIdentity,
&resp.requestingPortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH) == 0) &&
((ppi->sent_seq[PPM_PDELAY_REQ]) ==
hdr->sequenceId) &&
(DSPOR(ppi)->portIdentity.portNumber ==
resp.requestingPortIdentity.portNumber) &&
(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
to_TimeInternal(&ppi->t4, &resp.requestReceiptTimestamp);
ppi->t6 = ppi->last_rcv_time;
ppi->t6_cf = phase_to_cf_units(ppi->last_rcv_time.phase);
ppi->flags |= PPI_FLAG_WAITING_FOR_RF_UP;
/* todo: in one clock the presp carries t5-t4 */
} else {
pp_diag(ppi, frames, 2, "pp_pclock : "
"PDelay Resp doesn't match PDelay Req\n");
}
return 0;
}
int st_com_peer_handle_preq(struct pp_instance *ppi, unsigned char *buf,
int len)
{
if (len < PP_PDELAY_REQ_LENGTH)
return -1;
msg_copy_header(&ppi->pdelay_req_hdr, &ppi->received_ptp_header);
msg_issue_pdelay_resp(ppi, &ppi->last_rcv_time);
msg_issue_pdelay_resp_followup(ppi, &ppi->last_snt_time);
return 0;
}
/* Called by slave and uncalibrated */
int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
int len)
......
......@@ -35,6 +35,12 @@ int st_com_master_handle_sync(struct pp_instance *ppi, unsigned char *buf,
int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
int len);
int st_com_peer_handle_preq(struct pp_instance *ppi, unsigned char *buf,
int len);
int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
int len);
static inline int __send_and_log(struct pp_instance *ppi, int msglen,
int msgtype, int chtype)
{
......
......@@ -13,7 +13,6 @@ int pp_pclock(struct pp_instance *ppi, unsigned char *pkt, int plen)
int e = 0; /* error var, to check errors in msg handling */
MsgHeader *hdr = &ppi->received_ptp_header;
MsgPDelayResp resp;
MsgPDelayRespFollowUp respFllw;
int d1, d2;
......@@ -54,36 +53,19 @@ int pp_pclock(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (e)
break;
msg_copy_header(&ppi->pdelay_req_hdr,
&ppi->received_ptp_header);
msg_issue_pdelay_resp(ppi, &ppi->last_rcv_time);
msg_issue_pdelay_resp_followup(ppi, &ppi->last_snt_time);
break;
if (pp_hooks.handle_preq)
e = pp_hooks.handle_preq(ppi);
else
e = st_com_peer_handle_preq(ppi, pkt, plen);
case PPM_PDELAY_RESP:
e = (plen < PP_PDELAY_RESP_LENGTH);
if (e)
break;
msg_unpack_pdelay_resp(pkt, &resp);
goto out;
if ((memcmp(&DSPOR(ppi)->portIdentity.clockIdentity,
&resp.requestingPortIdentity.clockIdentity,
PP_CLOCK_IDENTITY_LENGTH) == 0) &&
((ppi->sent_seq[PPM_PDELAY_REQ]) ==
hdr->sequenceId) &&
(DSPOR(ppi)->portIdentity.portNumber ==
resp.requestingPortIdentity.portNumber) &&
(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
break;
to_TimeInternal(&ppi->t4,
&resp.requestReceiptTimestamp);
ppi->t6 = ppi->last_rcv_time;
case PPM_PDELAY_RESP:
} else {
pp_diag(ppi, frames, 2, "pp_pclock : "
"PDelay Resp doesn't match PDelay Req\n");
}
e = st_com_peer_handle_pres(ppi, pkt, plen);
break;
case PPM_PDELAY_RESP_FOLLOW_UP:
......@@ -104,12 +86,18 @@ int pp_pclock(struct pp_instance *ppi, unsigned char *pkt, int plen)
to_TimeInternal(&ppi->t5,
&respFllw.responseOriginTimestamp);
ppi->flags |= PPI_FLAG_WAITING_FOR_RF_UP;
pp_servo_got_presp(ppi);
if (pp_hooks.handle_presp)
e = pp_hooks.handle_presp(ppi);
else
pp_servo_got_presp(ppi);
if (e)
goto out;
} else {
pp_diag(ppi, frames, 2, "pp_pclock : "
"Delay Resp Follow doesn't match Delay Req\n");
"PDelay Resp Follow doesn't match PDelay Req\n");
}
break;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment