This patch adds a new "NSM" command to the pmc program. The new code
handles only one outstanding NSM command at a time. If and when all
four event time stamps have arrived, the code prints the instantaneous
estimated offset without any averaging or smoothing.
Signed-off-by: Richard Cochran <richardcoch...@gmail.com>
---
pmc.c | 28 +++++-
pmc_common.c | 324 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-
pmc_common.h | 20 ++++
3 files changed, 363 insertions(+), 9 deletions(-)
diff --git a/pmc.c b/pmc.c
index a15e800..c75b590 100644
--- a/pmc.c
+++ b/pmc.c
@@ -36,6 +36,7 @@
#include "version.h"
#define BAD_ACTION -1
+#define NSM_ACTION -2
#define BAD_ID -1
#define AMBIGUOUS_ID -2
#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
@@ -117,8 +118,6 @@ static const char *action_string[] = {
"ACKNOWLEDGE",
};
-#define IFMT "\n\t\t"
-
static char *text2str(struct PTPText *text)
{
static struct static_ptp_text s;
@@ -561,6 +560,8 @@ static int parse_action(char *s)
return COMMAND;
else if (0 == strncasecmp(s, "COMMAND", len))
return COMMAND;
+ else if (0 == strncasecmp(s, "NSM", len))
+ return NSM_ACTION;
return BAD_ACTION;
}
@@ -613,6 +614,11 @@ static void print_help(FILE *fp)
fprintf(fp, "\tTARGET [portIdentity]\n");
fprintf(fp, "\tTARGET *\n");
fprintf(fp, "\n");
+ fprintf(fp, "\tSend a NetSync Monitor request to a specific port
address:\n");
+ fprintf(fp, "\n");
+ fprintf(fp, "\tNSM 111.222.333.444\n");
+ fprintf(fp, "\tNSM aa.bb.cc.dd.ee.ff\n");
+ fprintf(fp, "\n");
}
static int do_command(char *str)
@@ -632,6 +638,9 @@ static int do_command(char *str)
return parse_target(id_str);
action = parse_action(action_str);
+ if (action == NSM_ACTION) {
+ return pmc_nsm_request(pmc, id_str);
+ }
id = parse_id(id_str);
if (action == BAD_ACTION || id == BAD_ID)
@@ -688,7 +697,7 @@ int main(int argc, char *argv[])
UInteger8 boundary_hops = 1, domain_number = 0, transport_specific = 0;
struct ptp_message *msg;
struct config *cfg;
-#define N_FD 2
+#define N_FD 3
struct pollfd pollfd[N_FD];
handle_term_signals();
@@ -794,6 +803,7 @@ int main(int argc, char *argv[])
pollfd[0].fd = batch_mode ? -1 : STDIN_FILENO;
pollfd[1].fd = pmc_get_transport_fd(pmc);
+ pollfd[2].fd = pmc_get_transport_event_fd(pmc);
while (is_running()) {
if (batch_mode && !command) {
@@ -808,6 +818,7 @@ int main(int argc, char *argv[])
pollfd[0].events = 0;
pollfd[1].events = POLLIN | POLLPRI;
+ pollfd[2].events = POLLIN | POLLPRI;
if (!batch_mode && !command)
pollfd[0].events |= POLLIN | POLLPRI;
@@ -856,7 +867,16 @@ int main(int argc, char *argv[])
if (pollfd[1].revents & (POLLIN|POLLPRI)) {
msg = pmc_recv(pmc);
if (msg) {
- pmc_show(msg, stdout);
+ if (!pmc_handle_nsm(pmc, msg, stdout)) {
+ pmc_show(msg, stdout);
+ }
+ msg_put(msg);
+ }
+ }
+ if (pollfd[2].revents & (POLLIN|POLLPRI)) {
+ msg = pmc_recv_event(pmc);
+ if (msg) {
+ pmc_handle_nsm(pmc, msg, stdout);
msg_put(msg);
}
}
diff --git a/pmc_common.c b/pmc_common.c
index da25c21..6366f76 100644
--- a/pmc_common.c
+++ b/pmc_common.c
@@ -17,9 +17,13 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
+#include <arpa/inet.h>
#include <errno.h>
-#include <string.h>
+#include <netinet/in.h>
+#include <stdint.h>
#include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
#include "print.h"
#include "tlv.h"
@@ -60,8 +64,75 @@ struct pmc {
struct fdarray fdarray;
struct tsproc *tsproc;
int zero_length_gets;
+
+ /* NetSync Monitor */
+ struct ptp_message *nsm_delay_req;
+ struct ptp_message *nsm_delay_resp;
+ struct ptp_message *nsm_sync;
+ struct ptp_message *nsm_fup;
+ enum timestamp_type timestamping;
+ Integer64 asymmetry;
};
+static int nsm_complete(struct pmc *pmc)
+{
+ if (!pmc->nsm_sync) {
+ return 0;
+ }
+ if (one_step(pmc->nsm_sync)) {
+ return pmc->nsm_delay_resp ? 1 : 0;
+ }
+ return (pmc->nsm_delay_resp && pmc->nsm_fup) ? 1 : 0;
+}
+
+static int64_t nsm_compute_offset(struct tsproc *tsp,
+ struct ptp_message *syn,
+ struct ptp_message *fup,
+ struct ptp_message *req,
+ struct ptp_message *resp)
+{
+ tmv_t c1, c2, c3, t1, t1c, t2, t3, t4, t4c, offset;
+
+ c1 = correction_to_tmv(syn->header.correction);
+ c2 = correction_to_tmv(fup->header.correction);
+ c3 = correction_to_tmv(resp->header.correction);
+
+ t1 = timestamp_to_tmv(fup->ts.pdu);
+ t2 = timespec_to_tmv(syn->hwts.ts);
+ t3 = timespec_to_tmv(req->hwts.ts);
+ t4 = timestamp_to_tmv(resp->ts.pdu);
+
+ t1c = tmv_add(t1, tmv_add(c1, c2));
+ t4c = tmv_sub(t4, c3);
+
+ tsproc_reset(tsp, 1);
+ tsproc_down_ts(tsp, t1c, t2);
+ tsproc_up_ts(tsp, t3, t4c);
+ tsproc_update_offset(tsp, &offset, NULL);
+
+ return tmv_to_nanoseconds(offset);
+}
+
+static void nsm_reset(struct pmc *pmc)
+{
+ if (pmc->nsm_delay_req) {
+ msg_put(pmc->nsm_delay_req);
+ }
+ if (pmc->nsm_delay_resp) {
+ msg_put(pmc->nsm_delay_resp);
+ }
+ if (pmc->nsm_sync) {
+ msg_put(pmc->nsm_sync);
+ }
+ if (pmc->nsm_fup) {
+ msg_put(pmc->nsm_fup);
+ }
+ pmc->nsm_delay_req = NULL;
+ pmc->nsm_delay_resp = NULL;
+ pmc->nsm_sync = NULL;
+ pmc->nsm_fup = NULL;
+}
+
struct pmc *pmc_create(struct config *cfg, enum transport_type transport_type,
const char *iface_name, UInteger8 boundary_hops,
UInteger8 domain_number, UInteger8 transport_specific,
@@ -107,6 +178,10 @@ struct pmc *pmc_create(struct config *cfg, enum
transport_type transport_type,
}
pmc->zero_length_gets = zero_datalen ? 1 : 0;
+ pmc->timestamping = config_get_int(cfg, NULL, "time_stamping");
+ pmc->asymmetry = config_get_int(cfg, iface_name, "delayAsymmetry");
+ pmc->asymmetry <<= 16;
+
return pmc;
failed:
@@ -120,6 +195,7 @@ failed:
void pmc_destroy(struct pmc *pmc)
{
+ nsm_reset(pmc);
tsproc_destroy(pmc->tsproc);
transport_close(pmc->transport, &pmc->fdarray);
transport_destroy(pmc->transport);
@@ -238,6 +314,228 @@ int pmc_get_transport_event_fd(struct pmc *pmc)
return pmc->fdarray.fd[FD_EVENT];
}
+int pmc_handle_nsm(struct pmc *pmc, struct ptp_message *msg, FILE *fp)
+{
+ struct nsm_resp_tlv_head *head;
+ struct nsm_resp_tlv_foot *foot;
+ struct timePropertiesDS *tp;
+ struct PortAddress *paddr;
+ struct currentDS cds;
+ struct parentDS *pds;
+ struct Timestamp ts;
+ unsigned char *ptr;
+ int64_t offset;
+
+ if (!pmc->nsm_delay_req) {
+ return 0;
+ }
+ if (msg->header.sequenceId !=
+ ntohs(pmc->nsm_delay_req->header.sequenceId)) {
+ return 0;
+ }
+ if (!(msg->header.flagField[0] & UNICAST)) {
+ return 0;
+ }
+
+ switch (msg_type(msg)) {
+ case SYNC:
+ if (!pmc->nsm_sync) {
+ pmc->nsm_sync = msg;
+ msg_get(msg);
+ }
+ break;
+ case FOLLOW_UP:
+ if (!pmc->nsm_fup) {
+ pmc->nsm_fup = msg;
+ msg_get(msg);
+ }
+ break;
+ case DELAY_RESP:
+ if (!pmc->nsm_delay_resp) {
+ pmc->nsm_delay_resp = msg;
+ msg_get(msg);
+ }
+ break;
+ case DELAY_REQ:
+ case PDELAY_REQ:
+ case PDELAY_RESP:
+ case PDELAY_RESP_FOLLOW_UP:
+ case ANNOUNCE:
+ case SIGNALING:
+ case MANAGEMENT:
+ return 0;
+ }
+
+ if (!nsm_complete(pmc)) {
+ return 1;
+ }
+
+ head = (struct nsm_resp_tlv_head *)
pmc->nsm_delay_resp->delay_resp.suffix;
+ paddr = &head->parent_addr;
+
+ ptr = (unsigned char *) head;
+ ptr += sizeof(*head) + paddr->addressLength;
+ foot = (struct nsm_resp_tlv_foot *) ptr;
+
+ pds = &foot->parent;
+ memcpy(&cds, &foot->current, sizeof(cds));
+ tp = &foot->timeprop;
+ memcpy(&ts, &foot->lastsync, sizeof(ts));
+
+ offset = nsm_compute_offset(pmc->tsproc, pmc->nsm_sync, pmc->nsm_fup,
+ pmc->nsm_delay_req, pmc->nsm_delay_resp);
+
+ fprintf(fp, "NSM MEASUREMENT COMPLETE"
+ IFMT "offset %" PRId64
+ IFMT "portState %s"
+ IFMT "parentPortAddress %hu %s\n",
+ offset,
+ ps_str[head->port_state],
+ head->parent_addr.networkProtocol,
+ portaddr2str(&head->parent_addr));
+ fprintf(fp, "\tparentDataset"
+ IFMT "parentPortIdentity %s"
+ IFMT "parentStats %hhu"
+ IFMT "observedParentOffsetScaledLogVariance 0x%04hx"
+ IFMT "observedParentClockPhaseChangeRate 0x%08x"
+ IFMT "grandmasterPriority1 %hhu"
+ IFMT "gm.ClockClass %hhu"
+ IFMT "gm.ClockAccuracy 0x%02hhx"
+ IFMT "gm.OffsetScaledLogVariance 0x%04hx"
+ IFMT "grandmasterPriority2 %hhu"
+ IFMT "grandmasterIdentity %s\n",
+ pid2str(&pds->parentPortIdentity),
+ pds->parentStats,
+ pds->observedParentOffsetScaledLogVariance,
+ pds->observedParentClockPhaseChangeRate,
+ pds->grandmasterPriority1,
+ pds->grandmasterClockQuality.clockClass,
+ pds->grandmasterClockQuality.clockAccuracy,
+ pds->grandmasterClockQuality.offsetScaledLogVariance,
+ pds->grandmasterPriority2,
+ cid2str(&pds->grandmasterIdentity));
+ fprintf(fp, "\tcurrentDataset"
+ IFMT "stepsRemoved %hd"
+ IFMT "offsetFromMaster %.1f"
+ IFMT "meanPathDelay %.1f\n",
+ cds.stepsRemoved, cds.offsetFromMaster / 65536.0,
+ cds.meanPathDelay / 65536.0);
+ fprintf(fp, "\ttimePropertiesDataset"
+ IFMT "currentUtcOffset %hd"
+ IFMT "leap61 %d"
+ IFMT "leap59 %d"
+ IFMT "currentUtcOffsetValid %d"
+ IFMT "ptpTimescale %d"
+ IFMT "timeTraceable %d"
+ IFMT "frequencyTraceable %d"
+ IFMT "timeSource 0x%02hhx\n",
+ tp->currentUtcOffset,
+ tp->flags & LEAP_61 ? 1 : 0,
+ tp->flags & LEAP_59 ? 1 : 0,
+ tp->flags & UTC_OFF_VALID ? 1 : 0,
+ tp->flags & PTP_TIMESCALE ? 1 : 0,
+ tp->flags & TIME_TRACEABLE ? 1 : 0,
+ tp->flags & FREQ_TRACEABLE ? 1 : 0,
+ tp->timeSource);
+ fprintf(fp, "\tlastSyncTimestamp %" PRId64 ".%09u\n",
+ ((uint64_t)ts.seconds_lsb) | (((uint64_t)ts.seconds_msb) << 32),
+ ts.nanoseconds);
+
+ fflush(fp);
+ nsm_reset(pmc);
+
+ return 1;
+}
+
+int pmc_nsm_request(struct pmc *pmc, char *target)
+{
+ enum transport_type type = transport_type(pmc->transport);
+ unsigned char mac[MAC_LEN];
+ struct in_addr ipv4_addr;
+ struct ptp_message *msg;
+ int cnt, err, pdulen;
+ struct address dst;
+ struct TLV *tlv;
+
+ memset(&dst, 0, sizeof(dst));
+
+ switch (type) {
+ case TRANS_UDS:
+ case TRANS_UDP_IPV6:
+ case TRANS_DEVICENET:
+ case TRANS_CONTROLNET:
+ case TRANS_PROFINET:
+ pr_err("sorry, NSM not support with this transport");
+ return -1;
+ case TRANS_UDP_IPV4:
+ if (!inet_aton(target, &ipv4_addr)) {
+ pr_err("bad IPv4 address");
+ return -1;
+ }
+ dst.sin.sin_family = AF_INET;
+ dst.sin.sin_addr = ipv4_addr;
+ dst.len = sizeof(dst.sin);
+ break;
+ case TRANS_IEEE_802_3:
+ if (str2mac(target, mac)) {
+ pr_err("bad Layer-2 address");
+ return -1;
+ }
+ dst.sll.sll_family = AF_PACKET;
+ dst.sll.sll_halen = MAC_LEN;
+ memcpy(&dst.sll.sll_addr, mac, MAC_LEN);
+ dst.len = sizeof(dst.sll);
+ break;
+ }
+
+ msg = msg_allocate();
+ if (!msg) {
+ return -1;
+ }
+ pdulen = sizeof(struct delay_req_msg) + sizeof(*tlv);
+ tlv = (struct TLV *) msg->delay_req.suffix;
+ tlv->type = TLV_PTPMON_REQ;
+ tlv->length = 0;
+ msg->tlv_count = 1;
+ msg->hwts.type = pmc->timestamping;
+
+ msg->header.tsmt = DELAY_REQ | pmc->transport_specific;
+ msg->header.ver = PTP_VERSION;
+ msg->header.messageLength = pdulen;
+ msg->header.domainNumber = pmc->domain_number;
+ msg->header.correction = -pmc->asymmetry;
+ msg->header.sourcePortIdentity = pmc->port_identity;
+ msg->header.sequenceId = pmc->sequence_id++;
+ msg->header.control = CTL_DELAY_REQ;
+ msg->header.logMessageInterval = 0x7f;
+
+ msg->address = dst;
+ msg->header.flagField[0] |= UNICAST;
+
+ err = msg_pre_send(msg);
+ if (err) {
+ pr_err("msg_pre_send failed");
+ goto out;
+ }
+ cnt = transport_sendto(pmc->transport, &pmc->fdarray, 1, msg);
+ if (cnt <= 0) {
+ pr_err("transport_sendto failed");
+ err = -1;
+ goto out;
+ }
+ if (msg_sots_missing(msg)) {
+ pr_err("missing timestamp on transmitted delay request");
+ err = -1;
+ goto out;
+ }
+ nsm_reset(pmc);
+ pmc->nsm_delay_req = msg;
+ return 0;
+out:
+ msg_put(msg);
+ return err;
+}
+
int pmc_send_get_action(struct pmc *pmc, int id)
{
int datalen, pdulen;
@@ -301,18 +599,24 @@ int pmc_send_set_action(struct pmc *pmc, int id, void
*data, int datasize)
return 0;
}
-struct ptp_message *pmc_recv(struct pmc *pmc)
+static struct ptp_message *pmc_recv_internal(struct pmc *pmc, int event)
{
struct ptp_message *msg;
- int cnt, err;
+ int cnt, err, fd;
msg = msg_allocate();
if (!msg) {
pr_err("low memory");
return NULL;
}
- msg->hwts.type = TS_SOFTWARE;
- cnt = transport_recv(pmc->transport, pmc_get_transport_fd(pmc), msg);
+ if (event) {
+ msg->hwts.type = pmc->timestamping;
+ fd = pmc_get_transport_event_fd(pmc);
+ } else {
+ msg->hwts.type = TS_SOFTWARE;
+ fd = pmc_get_transport_fd(pmc);
+ }
+ cnt = transport_recv(pmc->transport, fd, msg);
if (cnt <= 0) {
pr_err("recv message failed");
goto failed;
@@ -339,6 +643,16 @@ failed:
return NULL;
}
+struct ptp_message *pmc_recv(struct pmc *pmc)
+{
+ return pmc_recv_internal(pmc, 0);
+}
+
+struct ptp_message *pmc_recv_event(struct pmc *pmc)
+{
+ return pmc_recv_internal(pmc, 1);
+}
+
int pmc_target(struct pmc *pmc, struct PortIdentity *pid)
{
pmc->target = *pid;
diff --git a/pmc_common.h b/pmc_common.h
index d5ccb29..f951d34 100644
--- a/pmc_common.h
+++ b/pmc_common.h
@@ -25,6 +25,8 @@
#include "msg.h"
#include "transport.h"
+#define IFMT "\n\t\t"
+
struct pmc;
struct pmc *pmc_create(struct config *cfg, enum transport_type transport_type,
@@ -37,11 +39,29 @@ void pmc_destroy(struct pmc *pmc);
int pmc_get_transport_fd(struct pmc *pmc);
int pmc_get_transport_event_fd(struct pmc *pmc);
+/**
+ * Handle a NetSync Monitor message.
+ * @param pmc A pointer obtained with pmc_create().
+ * @param msg A message which may or may not be a NetSync Monitor message.
+ * @param fp An open file for logging.
+ * @return One (1) if the message was handled, zero (0) otherwise.
+ */
+int pmc_handle_nsm(struct pmc *pmc, struct ptp_message *msg, FILE *fp);
+
+/**
+ * Sends a NetSync Monitor delay request to a given node.
+ * @param pmc A pointer obtained with pmc_create().
+ * @param target The address of the node to measure.
+ * @return Zero on success, non-zero otherwise.
+ */
+int pmc_nsm_request(struct pmc *pmc, char *target);
+
int pmc_send_get_action(struct pmc *pmc, int id);
int pmc_send_set_action(struct pmc *pmc, int id, void *data, int datasize);
struct ptp_message *pmc_recv(struct pmc *pmc);
+struct ptp_message *pmc_recv_event(struct pmc *pmc);
int pmc_target(struct pmc *pmc, struct PortIdentity *pid);
void pmc_target_port(struct pmc *pmc, UInteger16 portNumber);
--
2.11.0
------------------------------------------------------------------------------
Check out the vibrant tech community on one of the world's most
engaging tech sites, Slashdot.org! http://sdm.link/slashdot
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
