diff --git a/.gitignore b/.gitignore index 68a4c3e..6d288ae 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ /*.o /.version /hwstamp_ctl +/nsm /phc2sys /pmc /ptp4l diff --git a/makefile b/makefile index f898336..796235b 100644 --- a/makefile +++ b/makefile @@ -22,13 +22,13 @@ CC = $(CROSS_COMPILE)gcc VER = -DVER=$(version) CFLAGS = -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS) LDLIBS = -lm -lrt $(EXTRA_LDFLAGS) -PRG = ptp4l pmc phc2sys hwstamp_ctl phc_ctl timemaster +PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \ filter.o fsm.o hash.o linreg.o mave.o mmedian.o msg.o ntpshm.o nullf.o phc.o \ pi.o port.o print.o ptp4l.o raw.o rtnl.o servo.o sk.o stats.o tlv.o \ transport.o tsproc.o udp.o udp6.o uds.o util.o version.o -OBJECTS = $(OBJ) hwstamp_ctl.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \ +OBJECTS = $(OBJ) hwstamp_ctl.o nsm.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \ sysoff.o timemaster.o SRC = $(OBJECTS:.o=.c) DEPEND = $(OBJECTS:.o=.d) @@ -46,6 +46,9 @@ all: $(PRG) ptp4l: $(OBJ) +nsm: config.o filter.o hash.o mave.o mmedian.o msg.o nsm.o print.o raw.o \ + rtnl.o sk.o transport.o tlv.o tsproc.o udp.o udp6.o uds.o util.o version.o + pmc: config.o hash.o msg.o pmc.o pmc_common.o print.o raw.o sk.o tlv.o \ transport.o udp.o udp6.o uds.o util.o version.o diff --git a/nsm.c b/nsm.c new file mode 100644 index 0000000..972a4ae --- /dev/null +++ b/nsm.c @@ -0,0 +1,627 @@ +/** + * @file nsm.c + * @brief NSM client program + * @note Copyright (C) 2018 Richard Cochran + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "config.h" +#include "print.h" +#include "rtnl.h" +#include "util.h" +#include "version.h" + +#define IFMT "\n\t\t" +#define NSM_NFD 3 + +struct nsm { + struct config *cfg; + struct fdarray fda; + struct transport *trp; + struct tsproc *tsproc; + struct ptp_message *nsm_delay_req; + struct ptp_message *nsm_delay_resp; + struct ptp_message *nsm_sync; + struct ptp_message *nsm_fup; + struct PortIdentity port_identity; + UInteger16 sequence_id; + const char *name; +} the_nsm; + +static void nsm_help(FILE *fp); +static int nsm_request(struct nsm *nsm, char *target); +static void nsm_reset(struct nsm *nsm); + +static int nsm_command(struct nsm *nsm, const char *cmd) +{ + char action_str[10+1] = {0}, id_str[64+1] = {0}; + + if (0 == strncasecmp(cmd, "HELP", strlen(cmd))) { + nsm_help(stdout); + return 0; + } + if (2 != sscanf(cmd, " %10s %64s", action_str, id_str)) { + pr_err("bad command: %s", cmd); + return -1; + } + if (0 == strncasecmp(action_str, "NSM", strlen(action_str))) { + return nsm_request(nsm, id_str); + } + pr_err("bad command: %s", cmd); + return -1; +} + +static int nsm_complete(struct nsm *nsm) +{ + if (!nsm->nsm_sync) { + return 0; + } + if (one_step(nsm->nsm_sync)) { + return nsm->nsm_delay_resp ? 1 : 0; + } + return (nsm->nsm_delay_resp && nsm->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_close(struct nsm *nsm) +{ + nsm_reset(nsm); + transport_close(nsm->trp, &nsm->fda); + transport_destroy(nsm->trp); + tsproc_destroy(nsm->tsproc); +} + +static void nsm_handle_msg(struct nsm *nsm, 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 (!nsm->nsm_delay_req) { + return; + } + if (msg->header.sequenceId != + ntohs(nsm->nsm_delay_req->header.sequenceId)) { + return; + } + if (!(msg->header.flagField[0] & UNICAST)) { + return; + } + + switch (msg_type(msg)) { + case SYNC: + if (!nsm->nsm_sync) { + nsm->nsm_sync = msg; + msg_get(msg); + } + break; + case FOLLOW_UP: + if (!nsm->nsm_fup) { + nsm->nsm_fup = msg; + msg_get(msg); + } + break; + case DELAY_RESP: + if (!nsm->nsm_delay_resp) { + nsm->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; + } + + if (!nsm_complete(nsm)) { + return; + } + + head = (struct nsm_resp_tlv_head *) nsm->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(nsm->tsproc, nsm->nsm_sync, nsm->nsm_fup, + nsm->nsm_delay_req, nsm->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(nsm); +} + +static void nsm_help(FILE *fp) +{ + 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 nsm_open(struct nsm *nsm, struct config *cfg) +{ + enum transport_type transport; + struct interface *iface; + const char *name; + int count = 0; + + STAILQ_FOREACH(iface, &cfg->interfaces, list) { + rtnl_get_ts_label(iface); + if (iface->ts_label[0] == '\0') { + strncpy(iface->ts_label, iface->name, MAX_IFNAME_SIZE); + } + count++; + } + if (count != 1) { + pr_err("need exactly one interface"); + return -1; + } + iface = STAILQ_FIRST(&cfg->interfaces); + nsm->name = name = iface->name; + nsm->cfg = cfg; + + transport = config_get_int(cfg, name, "network_transport"); + + if (generate_clock_identity(&nsm->port_identity.clockIdentity, name)) { + pr_err("failed to generate a clock identity"); + return -1; + } + nsm->port_identity.portNumber = 1; + + nsm->tsproc = tsproc_create(TSPROC_RAW, FILTER_MOVING_AVERAGE, 10); + if (!nsm->tsproc) { + pr_err("failed to create time stamp processor"); + goto no_tsproc; + } + nsm->trp = transport_create(cfg, transport); + if (!nsm->trp) { + pr_err("failed to create transport"); + goto no_trans; + } + if (transport_open(nsm->trp, iface, &nsm->fda, + config_get_int(cfg, NULL, "time_stamping"))) { + pr_err("failed to open transport"); + goto open_failed; + } + return 0; + +open_failed: + transport_destroy(nsm->trp); +no_trans: + tsproc_destroy(nsm->tsproc); +no_tsproc: + return -1; +} + +static struct ptp_message *nsm_recv(struct nsm *nsm, int fd) +{ + struct ptp_message *msg; + int cnt, err; + + msg = msg_allocate(); + if (!msg) { + pr_err("low memory"); + return NULL; + } + msg->hwts.type = config_get_int(nsm->cfg, NULL, "time_stamping"); + + cnt = transport_recv(nsm->trp, fd, msg); + if (cnt <= 0) { + pr_err("recv message failed"); + goto failed; + } + err = msg_post_recv(msg, cnt); + if (err) { + switch (err) { + case -EBADMSG: + pr_err("bad message"); + break; + case -ETIME: + pr_err("received %s without timestamp", + msg_type_string(msg_type(msg))); + break; + case -EPROTO: + pr_debug("ignoring message"); + break; + } + goto failed; + } + + return msg; +failed: + msg_put(msg); + return NULL; +} + +static int nsm_request(struct nsm *nsm, char *target) +{ + enum transport_type type = transport_type(nsm->trp); + UInteger8 transportSpecific; + unsigned char mac[MAC_LEN]; + struct in_addr ipv4_addr; + struct ptp_message *msg; + struct tlv_extra *extra; + Integer64 asymmetry; + struct address dst; + int cnt, err; + + 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; + } + + transportSpecific = config_get_int(nsm->cfg, nsm->name, "transportSpecific"); + transportSpecific <<= 4; + + asymmetry = config_get_int(nsm->cfg, nsm->name, "delayAsymmetry"); + asymmetry <<= 16; + + msg->hwts.type = config_get_int(nsm->cfg, NULL, "time_stamping"); + + msg->header.tsmt = DELAY_REQ | transportSpecific; + msg->header.ver = PTP_VERSION; + msg->header.messageLength = sizeof(struct delay_req_msg); + msg->header.domainNumber = config_get_int(nsm->cfg, NULL, "domainNumber"); + msg->header.correction = -asymmetry; + msg->header.sourcePortIdentity = nsm->port_identity; + msg->header.sequenceId = nsm->sequence_id++; + msg->header.control = CTL_DELAY_REQ; + msg->header.logMessageInterval = 0x7f; + + msg->address = dst; + msg->header.flagField[0] |= UNICAST; + + extra = msg_tlv_append(msg, sizeof(struct TLV)); + if (!extra) { + msg_put(msg); + return -ENOMEM; + } + extra->tlv->type = TLV_PTPMON_REQ; + extra->tlv->length = 0; + + err = msg_pre_send(msg); + if (err) { + pr_err("msg_pre_send failed"); + goto out; + } + cnt = transport_sendto(nsm->trp, &nsm->fda, 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(nsm); + nsm->nsm_delay_req = msg; + return 0; +out: + msg_put(msg); + return err; +} + +static void nsm_reset(struct nsm *nsm) +{ + if (nsm->nsm_delay_req) { + msg_put(nsm->nsm_delay_req); + } + if (nsm->nsm_delay_resp) { + msg_put(nsm->nsm_delay_resp); + } + if (nsm->nsm_sync) { + msg_put(nsm->nsm_sync); + } + if (nsm->nsm_fup) { + msg_put(nsm->nsm_fup); + } + nsm->nsm_delay_req = NULL; + nsm->nsm_delay_resp = NULL; + nsm->nsm_sync = NULL; + nsm->nsm_fup = NULL; +} + +static void usage(char *progname) +{ + fprintf(stderr, + "\nusage: %s [options]\n\n" + " -f [file] read configuration from 'file'\n" + " -h prints this message and exits\n" + " -i [dev] interface device to use\n" + " -v prints the software version and exits\n" + "\n", + progname); +} + +int main(int argc, char *argv[]) +{ + char *cmd = NULL, *config = NULL, line[1024], *progname; + int c, cnt, err = 0, index, length, tmo = -1; + struct pollfd pollfd[NSM_NFD]; + struct nsm *nsm = &the_nsm; + struct ptp_message *msg; + struct option *opts; + struct config *cfg; + + if (handle_term_signals()) { + return -1; + } + cfg = config_create(); + if (!cfg) { + return -1; + } + opts = config_long_options(cfg); + print_set_verbose(1); + print_set_syslog(0); + + /* Process the command line arguments. */ + progname = strrchr(argv[0], '/'); + progname = progname ? 1+progname : argv[0]; + while (EOF != (c = getopt_long(argc, argv, "f:hi:v", opts, &index))) { + switch (c) { + case 0: + if (config_parse_option(cfg, opts[index].name, optarg)) { + config_destroy(cfg); + return -1; + } + break; + case 'f': + config = optarg; + break; + case 'i': + if (!config_create_interface(optarg, cfg)) { + config_destroy(cfg); + return -1; + } + break; + case 'v': + version_show(stdout); + config_destroy(cfg); + return 0; + case 'h': + usage(progname); + config_destroy(cfg); + return 0; + case '?': + default: + usage(progname); + config_destroy(cfg); + return -1; + } + } + + print_set_syslog(0); + print_set_verbose(1); + + if (config && (err = config_read(config, cfg))) { + goto out; + } + + print_set_progname(progname); + print_set_tag(config_get_string(cfg, NULL, "message_tag")); + print_set_level(config_get_int(cfg, NULL, "logging_level")); + + err = nsm_open(nsm, cfg); + if (err) { + goto out; + } + pollfd[0].fd = nsm->fda.fd[0]; + pollfd[1].fd = nsm->fda.fd[1]; + pollfd[2].fd = STDIN_FILENO; + pollfd[0].events = POLLIN | POLLPRI; + pollfd[1].events = POLLIN | POLLPRI; + pollfd[2].events = POLLIN | POLLPRI; + + while (is_running()) { + cnt = poll(pollfd, NSM_NFD, tmo); + if (cnt < 0) { + if (EINTR == errno) { + continue; + } else { + pr_emerg("poll failed"); + err = -1; + break; + } + } else if (!cnt) { + break; + } + if (pollfd[2].revents & POLLHUP) { + if (tmo == -1) { + /* Wait a bit longer for outstanding replies. */ + tmo = 100; + pollfd[2].fd = -1; + pollfd[2].events = 0; + } else { + break; + } + } + if (pollfd[2].revents & (POLLIN|POLLPRI)) { + if (!fgets(line, sizeof(line), stdin)) { + break; + } + length = strlen(line); + if (length < 2) { + continue; + } + line[length - 1] = 0; + cmd = line; + if (nsm_command(nsm, cmd)) { + pr_err("command failed"); + } + } + if (pollfd[0].revents & (POLLIN|POLLPRI)) { + msg = nsm_recv(nsm, pollfd[0].fd); + if (msg) { + nsm_handle_msg(nsm, msg, stdout); + msg_put(msg); + } + } + if (pollfd[1].revents & (POLLIN|POLLPRI)) { + msg = nsm_recv(nsm, pollfd[1].fd); + if (msg) { + nsm_handle_msg(nsm, msg, stdout); + msg_put(msg); + } + } + } + + nsm_close(nsm); +out: + msg_cleanup(); + config_destroy(cfg); + return err; +}