Commit d17cb001 authored by Alessandro Rubini's avatar Alessandro Rubini

Merge branch 'rubi-misc'

This is old misc stuff that I rebased yesterday. Actually, I had more
moves to fsm-lib, but some of it already moved to common-fun (which I
dislike, but let's ignore the thing by now).
parents 4e0caa46 8cd4bda8
...@@ -84,12 +84,6 @@ void sim_main_loop(struct pp_globals *ppg) ...@@ -84,12 +84,6 @@ void sim_main_loop(struct pp_globals *ppg)
PP_MAX_FRAME_LENGTH - 4, PP_MAX_FRAME_LENGTH - 4,
&ppi->last_rcv_time); &ppi->last_rcv_time);
if (i < PP_MINIMUM_LENGTH) {
pp_diag(ppi, frames, 1, "Error or short frame: "
"%d < %d\n", i, PP_MINIMUM_LENGTH);
continue;
}
sim_set_global_DS(ppi); sim_set_global_DS(ppi);
tmp_ns = 1000LL * 1000LL * pp_state_machine(ppi, tmp_ns = 1000LL * 1000LL * pp_state_machine(ppi,
ppi->rx_ptp, i - ppi->rx_offset); ppi->rx_ptp, i - ppi->rx_offset);
......
...@@ -116,12 +116,6 @@ void unix_main_loop(struct pp_globals *ppg) ...@@ -116,12 +116,6 @@ void unix_main_loop(struct pp_globals *ppg)
errno, strerror(errno)); errno, strerror(errno));
continue; continue;
} }
if (i < PP_MINIMUM_LENGTH) {
pp_diag(ppi, frames, 1,
"Short frame: %d < %d\n", i,
PP_MINIMUM_LENGTH);
continue;
}
tmp_d = pp_state_machine(ppi, ppi->rx_ptp, tmp_d = pp_state_machine(ppi, ppi->rx_ptp,
i - ppi->rx_offset); i - ppi->rx_offset);
......
...@@ -157,12 +157,6 @@ void wrs_main_loop(struct pp_globals *ppg) ...@@ -157,12 +157,6 @@ void wrs_main_loop(struct pp_globals *ppg)
errno, strerror(errno)); errno, strerror(errno));
continue; continue;
} }
if (i < PP_MINIMUM_LENGTH) {
pp_diag(ppi, frames, 1,
"Short frame: %d < %d\n", i,
PP_MINIMUM_LENGTH);
continue;
}
tmp_d = pp_state_machine(ppi, ppi->rx_ptp, tmp_d = pp_state_machine(ppi, ppi->rx_ptp,
i - ppi->rx_offset); i - ppi->rx_offset);
......
...@@ -157,6 +157,33 @@ static int pp_packet_prefilter(struct pp_instance *ppi) ...@@ -157,6 +157,33 @@ static int pp_packet_prefilter(struct pp_instance *ppi)
return 0; return 0;
} }
/* used to make basic checks before the individual state is called */
static int type_length[__PP_NR_MESSAGES_TYPES] = {
[PPM_SYNC] = PP_SYNC_LENGTH,
[PPM_DELAY_REQ] = PP_DELAY_REQ_LENGTH,
[PPM_PDELAY_REQ] = PP_PDELAY_REQ_LENGTH,
[PPM_PDELAY_RESP] = PP_PDELAY_RESP_LENGTH,
[PPM_FOLLOW_UP] = PP_FOLLOW_UP_LENGTH,
[PPM_DELAY_RESP] = PP_DELAY_RESP_LENGTH,
[PPM_PDELAY_R_FUP] = PP_PDELAY_R_FUP_LENGTH,
[PPM_ANNOUNCE] = PP_ANNOUNCE_LENGTH,
[PPM_SIGNALING] = PP_HEADER_LENGTH,
[PPM_MANAGEMENT] = PP_MANAGEMENT_LENGTH,
};
static int fsm_unpack_verify_frame(struct pp_instance *ppi,
uint8_t *packet, int plen)
{
int msgtype;
msgtype = packet[0] & 0xf;
if (msgtype >= __PP_NR_MESSAGES_TYPES || plen < type_length[msgtype])
return 1; /* too short */
if ((packet[1] & 0xf) != 2)
return 1; /* wrong ptp version */
return msg_unpack_header(ppi, packet, plen);
}
/* /*
* This is the state machine code. i.e. the extension-independent * This is the state machine code. i.e. the extension-independent
* function that runs the machine. Errors are managed and reported * function that runs the machine. Errors are managed and reported
...@@ -190,14 +217,18 @@ int pp_state_machine(struct pp_instance *ppi, uint8_t *packet, int plen) ...@@ -190,14 +217,18 @@ int pp_state_machine(struct pp_instance *ppi, uint8_t *packet, int plen)
} }
/* /*
* Since all ptp frames have the same header, parse it now. * Since all ptp frames have the same header, parse it now,
* and centralize some error check.
* In case of error continue without a frame, so the current * In case of error continue without a frame, so the current
* ptp state can update ppi->next_delay and return a proper value * ptp state can update ppi->next_delay and return a proper value
*/ */
if (plen && msg_unpack_header(ppi, packet, plen)) { err = fsm_unpack_verify_frame(ppi, packet, plen);
packet = NULL; if (err) {
plen = 0; plen = 0;
packet = NULL;
} }
if (!plen)
ppi->received_ptp_header.messageType = PPM_NO_MESSAGE;
state = ppi->state; state = ppi->state;
......
...@@ -306,8 +306,9 @@ enum pp_std_messages { ...@@ -306,8 +306,9 @@ enum pp_std_messages {
PPM_SIGNALING, PPM_SIGNALING,
PPM_MANAGEMENT, PPM_MANAGEMENT,
__PP_NR_MESSAGES_TYPES, __PP_NR_MESSAGES_TYPES,
/* NO_MESSAGE means "no message received", or "eaten by hook" */
PPM_NO_MESSAGE,
PPM_NOTHING_TO_DO = 0x100, /* for hooks.master_msg() */
}; };
/* Enumeration Domain Number (table 2, page 41) */ /* Enumeration Domain Number (table 2, page 41) */
......
...@@ -63,6 +63,8 @@ extern struct pp_msgtype_info pp_msgtype_info[16]; ...@@ -63,6 +63,8 @@ extern struct pp_msgtype_info pp_msgtype_info[16];
extern int pp_lib_may_issue_sync(struct pp_instance *ppi); extern int pp_lib_may_issue_sync(struct pp_instance *ppi);
extern int pp_lib_may_issue_announce(struct pp_instance *ppi); extern int pp_lib_may_issue_announce(struct pp_instance *ppi);
extern int pp_lib_may_issue_request(struct pp_instance *ppi); extern int pp_lib_may_issue_request(struct pp_instance *ppi);
extern int pp_lib_handle_announce(struct pp_instance *ppi,
unsigned char *buf, int len);
/* We use data sets a lot, so have these helpers */ /* We use data sets a lot, so have these helpers */
static inline struct pp_globals *GLBS(struct pp_instance *ppi) static inline struct pp_globals *GLBS(struct pp_instance *ppi)
...@@ -151,7 +153,7 @@ struct pp_ext_hooks { ...@@ -151,7 +153,7 @@ struct pp_ext_hooks {
int (*handle_resp)(struct pp_instance *ppi); int (*handle_resp)(struct pp_instance *ppi);
void (*s1)(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann); void (*s1)(struct pp_instance *ppi, MsgHeader *hdr, MsgAnnounce *ann);
int (*execute_slave)(struct pp_instance *ppi); int (*execute_slave)(struct pp_instance *ppi);
void (*handle_announce)(struct pp_instance *ppi); int (*handle_announce)(struct pp_instance *ppi);
int (*handle_followup)(struct pp_instance *ppi, TimeInternal *orig, int (*handle_followup)(struct pp_instance *ppi, TimeInternal *orig,
TimeInternal *correction_field); TimeInternal *correction_field);
int (*handle_preq) (struct pp_instance * ppi); int (*handle_preq) (struct pp_instance * ppi);
......
...@@ -86,7 +86,8 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen, ...@@ -86,7 +86,8 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
MsgSignaling wrsig_msg; MsgSignaling wrsig_msg;
TimeInternal *time = &ppi->last_rcv_time; TimeInternal *time = &ppi->last_rcv_time;
pp_diag(ppi, ext, 2, "hook: %s\n", __func__); if (msgtype != PPM_NO_MESSAGE)
pp_diag(ppi, ext, 2, "hook: %s\n", __func__);
switch (msgtype) { switch (msgtype) {
...@@ -96,7 +97,7 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen, ...@@ -96,7 +97,7 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
hdr->correctionfield.lsb = hdr->correctionfield.lsb =
phase_to_cf_units(ppi->last_rcv_time.phase); phase_to_cf_units(ppi->last_rcv_time.phase);
msg_issue_delay_resp(ppi, time); /* no error check */ msg_issue_delay_resp(ppi, time); /* no error check */
msgtype = PPM_NOTHING_TO_DO; msgtype = PPM_NO_MESSAGE;
break; break;
case PPM_PDELAY_REQ: case PPM_PDELAY_REQ:
...@@ -114,7 +115,7 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen, ...@@ -114,7 +115,7 @@ static int wr_master_msg(struct pp_instance *ppi, unsigned char *pkt, int plen,
/* We must start the handshake as a WR master */ /* We must start the handshake as a WR master */
wr_handshake_init(ppi, PPS_MASTER); wr_handshake_init(ppi, PPS_MASTER);
} }
msgtype = PPM_NOTHING_TO_DO; msgtype = PPM_NO_MESSAGE;
break; break;
} }
...@@ -191,7 +192,7 @@ static int wr_execute_slave(struct pp_instance *ppi) ...@@ -191,7 +192,7 @@ static int wr_execute_slave(struct pp_instance *ppi)
return 1; /* the caller returns too */ return 1; /* the caller returns too */
} }
static void wr_handle_announce(struct pp_instance *ppi) static int wr_handle_announce(struct pp_instance *ppi)
{ {
pp_diag(ppi, ext, 2, "hook: %s\n", __func__); pp_diag(ppi, ext, 2, "hook: %s\n", __func__);
if ((WR_DSPOR(ppi)->wrConfig & WR_S_ONLY) && if ((WR_DSPOR(ppi)->wrConfig & WR_S_ONLY) &&
...@@ -201,6 +202,7 @@ static void wr_handle_announce(struct pp_instance *ppi) ...@@ -201,6 +202,7 @@ static void wr_handle_announce(struct pp_instance *ppi)
/* We must start the handshake as a WR slave */ /* We must start the handshake as a WR slave */
wr_handshake_init(ppi, PPS_SLAVE); wr_handshake_init(ppi, PPS_SLAVE);
} }
return 0;
} }
static int wr_handle_followup(struct pp_instance *ppi, static int wr_handle_followup(struct pp_instance *ppi,
......
...@@ -31,9 +31,6 @@ int wr_calibrated(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -31,9 +31,6 @@ int wr_calibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
return 0; /* non-wr */ return 0; /* non-wr */
} }
if (plen == 0)
goto out;
if (ppi->received_ptp_header.messageType == PPM_SIGNALING) { if (ppi->received_ptp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg, msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
&(wrp->msgTmpWrMessageID)); &(wrp->msgTmpWrMessageID));
...@@ -46,7 +43,6 @@ int wr_calibrated(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -46,7 +43,6 @@ int wr_calibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
ppi->next_state = WRS_WR_LINK_ON; ppi->next_state = WRS_WR_LINK_ON;
} }
out:
ppi->next_delay = wrp->wrStateTimeout; ppi->next_delay = wrp->wrStateTimeout;
return 0; return 0;
} }
...@@ -34,9 +34,6 @@ int wr_locked(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -34,9 +34,6 @@ int wr_locked(struct pp_instance *ppi, unsigned char *pkt, int plen)
e = msg_issue_wrsig(ppi, LOCKED); e = msg_issue_wrsig(ppi, LOCKED);
} }
if (plen == 0)
goto out;
if (ppi->received_ptp_header.messageType == PPM_SIGNALING) { if (ppi->received_ptp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg, msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
...@@ -46,7 +43,6 @@ int wr_locked(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -46,7 +43,6 @@ int wr_locked(struct pp_instance *ppi, unsigned char *pkt, int plen)
ppi->next_state = WRS_RESP_CALIB_REQ; ppi->next_state = WRS_RESP_CALIB_REQ;
} }
out:
if (e != 0) if (e != 0)
ppi->next_state = PPS_FAULTY; ppi->next_state = PPS_FAULTY;
ppi->next_delay = wrp->wrStateTimeout; ppi->next_delay = wrp->wrStateTimeout;
......
...@@ -34,9 +34,6 @@ int wr_m_lock(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -34,9 +34,6 @@ int wr_m_lock(struct pp_instance *ppi, unsigned char *pkt, int plen)
__pp_timeout_set(ppi, PP_TO_EXT_0, WR_M_LOCK_TIMEOUT_MS); __pp_timeout_set(ppi, PP_TO_EXT_0, WR_M_LOCK_TIMEOUT_MS);
} }
if (plen == 0)
goto out;
if (ppi->received_ptp_header.messageType == PPM_SIGNALING) { if (ppi->received_ptp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg, msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
...@@ -46,7 +43,6 @@ int wr_m_lock(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -46,7 +43,6 @@ int wr_m_lock(struct pp_instance *ppi, unsigned char *pkt, int plen)
ppi->next_state = WRS_CALIBRATION; ppi->next_state = WRS_CALIBRATION;
} }
out:
if (e != 0) if (e != 0)
ppi->next_state = PPS_FAULTY; ppi->next_state = PPS_FAULTY;
ppi->next_delay = wrp->wrStateTimeout; ppi->next_delay = wrp->wrStateTimeout;
......
...@@ -38,9 +38,6 @@ int wr_present(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -38,9 +38,6 @@ int wr_present(struct pp_instance *ppi, unsigned char *pkt, int plen)
e = msg_issue_wrsig(ppi, SLAVE_PRESENT); e = msg_issue_wrsig(ppi, SLAVE_PRESENT);
} }
if (plen == 0)
goto out;
if (ppi->received_ptp_header.messageType == PPM_SIGNALING) { if (ppi->received_ptp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg, msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
...@@ -50,7 +47,6 @@ int wr_present(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -50,7 +47,6 @@ int wr_present(struct pp_instance *ppi, unsigned char *pkt, int plen)
ppi->next_state = WRS_S_LOCK; ppi->next_state = WRS_S_LOCK;
} }
out:
if (e == 0) if (e == 0)
st_com_execute_slave(ppi); st_com_execute_slave(ppi);
else else
......
...@@ -35,9 +35,6 @@ int wr_resp_calib_req(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -35,9 +35,6 @@ int wr_resp_calib_req(struct pp_instance *ppi, unsigned char *pkt, int plen)
wrp->otherNodeCalPeriod / 1000); wrp->otherNodeCalPeriod / 1000);
} }
if (plen == 0)
goto out;
if (ppi->received_ptp_header.messageType == PPM_SIGNALING) { if (ppi->received_ptp_header.messageType == PPM_SIGNALING) {
msg_unpack_wrsig(ppi, pkt, &wrsig_msg, msg_unpack_wrsig(ppi, pkt, &wrsig_msg,
...@@ -53,7 +50,6 @@ int wr_resp_calib_req(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -53,7 +50,6 @@ int wr_resp_calib_req(struct pp_instance *ppi, unsigned char *pkt, int plen)
} }
} }
out:
ppi->next_delay = wrp->wrStateTimeout; ppi->next_delay = wrp->wrStateTimeout;
return e; return e;
} }
...@@ -100,66 +100,6 @@ int st_com_execute_slave(struct pp_instance *ppi) ...@@ -100,66 +100,6 @@ int st_com_execute_slave(struct pp_instance *ppi)
return 0; return 0;
} }
/* Called by this file, basically when an announce is got, all states */
static void st_com_add_foreign(struct pp_instance *ppi, unsigned char *buf)
{
int i;
MsgHeader *hdr = &ppi->received_ptp_header;
/* Check if foreign master is already known */
for (i = 0; i < ppi->frgn_rec_num; i++) {
if (!memcmp(&hdr->sourcePortIdentity,
&ppi->frgn_master[i].port_id,
sizeof(hdr->sourcePortIdentity))) {
/* already in Foreign master data set, update info */
msg_copy_header(&ppi->frgn_master[i].hdr, hdr);
msg_unpack_announce(buf, &ppi->frgn_master[i].ann);
return;
}
}
/* New foreign master */
if (ppi->frgn_rec_num < PP_NR_FOREIGN_RECORDS)
ppi->frgn_rec_num++;
/* FIXME: replace the worst */
i = ppi->frgn_rec_num - 1;
/* Copy new foreign master data set from announce message */
memcpy(&ppi->frgn_master[i].port_id,
&hdr->sourcePortIdentity, sizeof(hdr->sourcePortIdentity));
/*
* header and announce field of each Foreign Master are
* useful to run Best Master Clock Algorithm
*/
msg_copy_header(&ppi->frgn_master[i].hdr, hdr);
msg_unpack_announce(buf, &ppi->frgn_master[i].ann);
pp_diag(ppi, bmc, 1, "New foreign Master %i added\n", i);
}
/* Called by slave and uncalibrated */
int st_com_slave_handle_announce(struct pp_instance *ppi, unsigned char *buf,
int len)
{
if (len < PP_ANNOUNCE_LENGTH)
return -1;
/* st_com_add_foreign takes care of announce unpacking */
st_com_add_foreign(ppi, buf);
/*Reset Timer handling Announce receipt timeout*/
pp_timeout_set(ppi, PP_TO_ANN_RECEIPT);
ppi->next_state = bmc(ppi); /* got a new announce: run bmc */
if (pp_hooks.handle_announce)
pp_hooks.handle_announce(ppi);
return 0;
}
/* Called by slave and uncalibrated */ /* Called by slave and uncalibrated */
int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf, int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
...@@ -168,8 +108,6 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf, ...@@ -168,8 +108,6 @@ int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
MsgHeader *hdr = &ppi->received_ptp_header; MsgHeader *hdr = &ppi->received_ptp_header;
MsgSync sync; MsgSync sync;
if (len < PP_SYNC_LENGTH)
return -1;
if (!(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) if (!(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT))
return 0; return 0;
...@@ -200,9 +138,6 @@ int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf, ...@@ -200,9 +138,6 @@ int st_com_peer_handle_pres(struct pp_instance *ppi, unsigned char *buf,
MsgHeader *hdr = &ppi->received_ptp_header; MsgHeader *hdr = &ppi->received_ptp_header;
int e = 0; int e = 0;
if (len < PP_PDELAY_RESP_LENGTH)
return -1;
msg_unpack_pdelay_resp(buf, &resp); msg_unpack_pdelay_resp(buf, &resp);
if ((memcmp(&DSPOR(ppi)->portIdentity.clockIdentity, if ((memcmp(&DSPOR(ppi)->portIdentity.clockIdentity,
...@@ -294,9 +229,6 @@ int st_com_peer_handle_preq(struct pp_instance *ppi, unsigned char *buf, ...@@ -294,9 +229,6 @@ int st_com_peer_handle_preq(struct pp_instance *ppi, unsigned char *buf,
{ {
int e = 0; int e = 0;
if (len < PP_PDELAY_REQ_LENGTH)
return -1;
if (pp_hooks.handle_preq) if (pp_hooks.handle_preq)
e = pp_hooks.handle_preq(ppi); e = pp_hooks.handle_preq(ppi);
if (e) if (e)
...@@ -318,9 +250,6 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf, ...@@ -318,9 +250,6 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
MsgHeader *hdr = &ppi->received_ptp_header; MsgHeader *hdr = &ppi->received_ptp_header;
if (len < PP_FOLLOW_UP_LENGTH)
return -1;
if (!(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) { if (!(ppi->flags & PPI_FLAG_FROM_CURRENT_PARENT)) {
pp_error("%s: Follow up message is not from current parent\n", pp_error("%s: Follow up message is not from current parent\n",
__func__); __func__);
...@@ -363,24 +292,6 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf, ...@@ -363,24 +292,6 @@ int st_com_slave_handle_followup(struct pp_instance *ppi, unsigned char *buf,
return 0; return 0;
} }
/* Called by master, listenting, passive. */
int st_com_master_handle_announce(struct pp_instance *ppi, unsigned char *buf,
int len)
{
if (len < PP_ANNOUNCE_LENGTH)
return -1;
pp_diag(ppi, bmc, 2, "Announce message from another foreign master\n");
st_com_add_foreign(ppi, buf);
ppi->next_state = bmc(ppi); /* got a new announce: run bmc */
if (pp_hooks.handle_announce)
pp_hooks.handle_announce(ppi);
return 0;
}
/* /*
* Called by master, listenting, passive. * Called by master, listenting, passive.
* FIXME: this must be implemented to support one-step masters * FIXME: this must be implemented to support one-step masters
......
...@@ -20,12 +20,6 @@ int st_com_execute_slave(struct pp_instance *ppi); ...@@ -20,12 +20,6 @@ int st_com_execute_slave(struct pp_instance *ppi);
* message, -1 in case the message contained in buf is not proper (e.g. size * message, -1 in case the message contained in buf is not proper (e.g. size
* is not the expected one * is not the expected one
*/ */
int st_com_slave_handle_announce(struct pp_instance *ppi, unsigned char *buf,
int len);
int st_com_master_handle_announce(struct pp_instance *ppi, unsigned char *buf,
int len);
int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf, int st_com_slave_handle_sync(struct pp_instance *ppi, unsigned char *buf,
int len); int len);
......
...@@ -2,13 +2,12 @@ ...@@ -2,13 +2,12 @@
* Copyright (C) 2011 CERN (www.cern.ch) * Copyright (C) 2011 CERN (www.cern.ch)
* Author: Aurelio Colosimo * Author: Aurelio Colosimo
* Copyright (C) 2014 GSI (www.gsi.de) * Copyright (C) 2014 GSI (www.gsi.de)
* Author: Alessandro Rubin * Author: Alessandro Rubini
* *
* Released according to the GNU LGPL, version 2.1 or any later version. * Released according to the GNU LGPL, version 2.1 or any later version.
*/ */
#include <ppsi/ppsi.h> #include <ppsi/ppsi.h>
/* Local functions that build to nothing when Kconfig selects 0/1 vlans */ /* Local functions that build to nothing when Kconfig selects 0/1 vlans */
static int pp_vlan_issue_announce(struct pp_instance *ppi) static int pp_vlan_issue_announce(struct pp_instance *ppi)
{ {
...@@ -108,3 +107,54 @@ int pp_lib_may_issue_request(struct pp_instance *ppi) ...@@ -108,3 +107,54 @@ int pp_lib_may_issue_request(struct pp_instance *ppi)
ppi->t3 = ppi->last_snt_time; ppi->t3 = ppi->last_snt_time;
return 0; return 0;
} }
/* Called by this file, basically when an announce is got, all states */
static void __lib_add_foreign(struct pp_instance *ppi, unsigned char *buf)
{
int i;
MsgHeader *hdr = &ppi->received_ptp_header;
/* Check if foreign master is already known */
for (i = 0; i < ppi->frgn_rec_num; i++) {
if (!memcmp(&hdr->sourcePortIdentity,
&ppi->frgn_master[i].port_id,
sizeof(hdr->sourcePortIdentity))) {
/* already in Foreign master data set, update info */
msg_copy_header(&ppi->frgn_master[i].hdr, hdr);
msg_unpack_announce(buf, &ppi->frgn_master[i].ann);
return;
}
}
/* New foreign master */
if (ppi->frgn_rec_num < PP_NR_FOREIGN_RECORDS)
ppi->frgn_rec_num++;
/* FIXME: replace the worst */
i = ppi->frgn_rec_num - 1;
/* Copy new foreign master data set from announce message */
memcpy(&ppi->frgn_master[i].port_id,
&hdr->sourcePortIdentity, sizeof(hdr->sourcePortIdentity));
/*
* header and announce field of each Foreign Master are
* useful to run Best Master Clock Algorithm
*/
msg_copy_header(&ppi->frgn_master[i].hdr, hdr);
msg_unpack_announce(buf, &ppi->frgn_master[i].ann);
pp_diag(ppi, bmc, 1, "New foreign Master %i added\n", i);
}
int pp_lib_handle_announce(struct pp_instance *ppi, unsigned char *buf, int len)
{
__lib_add_foreign(ppi, buf);
ppi->next_state = bmc(ppi); /* got a new announce: run bmc */
pp_timeout_set(ppi, PP_TO_ANN_RECEIPT);
if (pp_hooks.handle_announce)
return pp_hooks.handle_announce(ppi);
return 0;
}
...@@ -22,13 +22,10 @@ int pp_listening(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -22,13 +22,10 @@ int pp_listening(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH) if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH)
e = pp_lib_may_issue_request(ppi); e = pp_lib_may_issue_request(ppi);
if (plen == 0)
goto out;
switch (ppi->received_ptp_header.messageType) { switch (ppi->received_ptp_header.messageType) {
case PPM_ANNOUNCE: case PPM_ANNOUNCE:
e = st_com_master_handle_announce(ppi, pkt, plen); e = pp_lib_handle_announce(ppi, pkt, plen);
break; break;
case PPM_SYNC: case PPM_SYNC:
......
...@@ -22,7 +22,7 @@ static pp_action *actions[] = { ...@@ -22,7 +22,7 @@ static pp_action *actions[] = {
#endif #endif
[PPM_FOLLOW_UP] = 0, [PPM_FOLLOW_UP] = 0,
[PPM_DELAY_RESP] = 0, [PPM_DELAY_RESP] = 0,
[PPM_ANNOUNCE] = st_com_master_handle_announce, [PPM_ANNOUNCE] = pp_lib_handle_announce,
/* skip signaling and management, for binary size */ /* skip signaling and management, for binary size */
}; };
...@@ -65,13 +65,10 @@ int pp_master(struct pp_instance *ppi, uint8_t *pkt, int plen) ...@@ -65,13 +65,10 @@ int pp_master(struct pp_instance *ppi, uint8_t *pkt, int plen)
else /* please check commit '6d7bf7e3' about below, I'm not sure */ else /* please check commit '6d7bf7e3' about below, I'm not sure */
pp_timeout_set(ppi, PP_TO_REQUEST); pp_timeout_set(ppi, PP_TO_REQUEST);
if (plen == 0)
goto out;
/* /*
* An extension can do special treatment of this message type, * An extension can do special treatment of this message type,
* possibly returning error or eating the message by returning * possibly returning error or eating the message by returning
* PPM_NOTHING_TO_DO * PPM_NO_MESSAGE
*/ */
msgtype = ppi->received_ptp_header.messageType; msgtype = ppi->received_ptp_header.messageType;
if (pp_hooks.master_msg) if (pp_hooks.master_msg)
...@@ -86,7 +83,7 @@ int pp_master(struct pp_instance *ppi, uint8_t *pkt, int plen) ...@@ -86,7 +83,7 @@ int pp_master(struct pp_instance *ppi, uint8_t *pkt, int plen)
/* /*
* The management of messages is now table-driven * The management of messages is now table-driven
*/ */
if (plen && msgtype < ARRAY_SIZE(actions) if (msgtype < ARRAY_SIZE(actions)
&& actions[msgtype]) { && actions[msgtype]) {
e = actions[msgtype](ppi, pkt, plen); e = actions[msgtype](ppi, pkt, plen);
} else { } else {
......
...@@ -17,13 +17,10 @@ int pp_passive(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -17,13 +17,10 @@ int pp_passive(struct pp_instance *ppi, unsigned char *pkt, int plen)
if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH) if (CONFIG_HAS_P2P && ppi->mech == PP_P2P_MECH)
e = pp_lib_may_issue_request(ppi); e = pp_lib_may_issue_request(ppi);
if (plen == 0)
goto no_incoming_msg;
switch (ppi->received_ptp_header.messageType) { switch (ppi->received_ptp_header.messageType) {
case PPM_ANNOUNCE: case PPM_ANNOUNCE:
e = st_com_master_handle_announce(ppi, pkt, plen); e = pp_lib_handle_announce(ppi, pkt, plen);
break; break;
case PPM_SYNC: case PPM_SYNC:
...@@ -51,7 +48,6 @@ int pp_passive(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -51,7 +48,6 @@ int pp_passive(struct pp_instance *ppi, unsigned char *pkt, int plen)
} }
no_incoming_msg:
if (e == 0) if (e == 0)
e = st_com_execute_slave(ppi); e = st_com_execute_slave(ppi);
......
...@@ -24,7 +24,7 @@ static pp_action *actions[] = { ...@@ -24,7 +24,7 @@ static pp_action *actions[] = {
#endif #endif
[PPM_FOLLOW_UP] = st_com_slave_handle_followup, [PPM_FOLLOW_UP] = st_com_slave_handle_followup,
[PPM_DELAY_RESP] = slave_handle_response, [PPM_DELAY_RESP] = slave_handle_response,
[PPM_ANNOUNCE] = st_com_slave_handle_announce, [PPM_ANNOUNCE] = pp_lib_handle_announce,
/* skip signaling and management, for binary size */ /* skip signaling and management, for binary size */
}; };
...@@ -35,9 +35,6 @@ static int slave_handle_response(struct pp_instance *ppi, unsigned char *pkt, ...@@ -35,9 +35,6 @@ static int slave_handle_response(struct pp_instance *ppi, unsigned char *pkt,
MsgHeader *hdr = &ppi->received_ptp_header; MsgHeader *hdr = &ppi->received_ptp_header;
MsgDelayResp resp; MsgDelayResp resp;
if (plen < PP_DELAY_RESP_LENGTH)
return 0;
msg_unpack_delay_resp(pkt, &resp); msg_unpack_delay_resp(pkt, &resp);
if ((memcmp(&DSPOR(ppi)->portIdentity.clockIdentity, if ((memcmp(&DSPOR(ppi)->portIdentity.clockIdentity,
...@@ -95,7 +92,7 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -95,7 +92,7 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
/* /*
* The management of messages is now table-driven * The management of messages is now table-driven
*/ */
if (plen && hdr->messageType < ARRAY_SIZE(actions) if (hdr->messageType < ARRAY_SIZE(actions)
&& actions[hdr->messageType]) { && actions[hdr->messageType]) {
e = actions[hdr->messageType](ppi, pkt, plen); e = actions[hdr->messageType](ppi, pkt, plen);
} else { } else {
......
...@@ -13,13 +13,10 @@ int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -13,13 +13,10 @@ int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
{ {
int e = 0; /* error var, to check errors in msg handling */ int e = 0; /* error var, to check errors in msg handling */
if (plen == 0)
goto no_incoming_msg;
switch (ppi->received_ptp_header.messageType) { switch (ppi->received_ptp_header.messageType) {
case PPM_ANNOUNCE: case PPM_ANNOUNCE:
e = st_com_slave_handle_announce(ppi, pkt, plen); e = pp_lib_handle_announce(ppi, pkt, plen);
break; break;
case PPM_SYNC: case PPM_SYNC:
...@@ -35,7 +32,6 @@ int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen) ...@@ -35,7 +32,6 @@ int pp_uncalibrated(struct pp_instance *ppi, unsigned char *pkt, int plen)
break; break;
} }
no_incoming_msg:
if (e == 0) if (e == 0)
e = st_com_execute_slave(ppi); e = st_com_execute_slave(ppi);
......
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