From 66a9f902d10d4d1f4d6d65d95e8074b9acad4e0e Mon Sep 17 00:00:00 2001 From: Casper Andersson Date: Wed, 22 Nov 2023 13:14:32 +0100 Subject: [PATCH] pkt: Lots of improvements. Refactored. Added sequences of types. `-T` argument can now be repeated to create a sequence of different types. Signed-off-by: Casper Andersson --- src/pkt.c | 255 +++++++++++++++++++++++++++++++++--------------------- 1 file changed, 155 insertions(+), 100 deletions(-) diff --git a/src/pkt.c b/src/pkt.c index 227c761..a51afdd 100644 --- a/src/pkt.c +++ b/src/pkt.c @@ -35,6 +35,8 @@ #define SIOCSHWTSTAMP 0x89b0 #endif +#define SEQUENCE_MAX 100 + extern int debugen; struct pkt_cfg { @@ -46,72 +48,167 @@ struct pkt_cfg { int twoStepFlag; int tstamp_all; int auto_fup; - int ptp_type; int onestep; int rx_only; int version; + int listen; int domain; int tstype; int count; - int prio; int seq; char mac[ETH_ALEN]; char *interface; + int sequence_types[SEQUENCE_MAX]; + int sequence_length; }; void pkt_help() { fprintf(stderr, "\n--- TSTest Packets ---\n\n"); - fprintf(stderr, "Transmits and receives PTP packets and outputs the timestamps.\n\n\ + fprintf(stderr, "Transmits and receives PTP packets and outputs the timestamps\n\n\ Usage:\n\ tstest pkt [options]\n\n\ Options:\n\ - -i \n\ - -T \n\ - -t TX only mode \n\ - -r RX only mode \n\ - -a Timestamp all packets (CURRENTLY NOT WORKING) \n\ - -D Domain \n\ - -o Use one-step timestamping \n\ - -O Use p2p-one-step timestamping \n\ - -s \n\ - -m \n\ - -c \n\ - -p \n\ + -i \n\ + -T . Can be repeated to create a sequence of packets\n\ + -r Only listen to incoming packets\n\ + -a Timestamp all packets (CURRENTLY NOT WORKING)\n\ + -D . PTP domain number\n\ + -o Use one-step timestamping\n\ + -O Use p2p-one-step timestamping\n\ + -s . Start PTP sequence ID at this value.\n\ + -m \n\ + -c . Set to 0 to send until stopped\n\ + -l <0|1>. 0: Never fetch timestamp. 1: Always fetch timestamp (even for types that might not have)\n\ -d Enable debug output\n\ -h help\n\ + --transportSpecific . Set value for the transportSpecific field\n\ + --twoStepFlag <0|1>. Force if twoStepFlag should be set or not. Default is automatic\n\ \n"); } -int send_auto_fup(struct pkt_cfg *cfg, int sock) +void set_two_step_flag(struct pkt_cfg *cfg, struct ptp_header *hdr, int type) { - struct ptp_header hdr; - union Message msg; - int ptp_type; - int err; + int twoStepFlag = 0x00; + + if (cfg->twoStepFlag_set) { + // Manually control two-step flag + if (cfg->twoStepFlag) + twoStepFlag = 0x02; + else + twoStepFlag = 0x00; + } else { + if (type == SYNC && cfg->tstype != TS_ONESTEP && cfg->tstype != TS_P2P1STEP) + twoStepFlag = 0x02; + else if (type == PDELAY_RESP && cfg->tstype != TS_P2P1STEP) + twoStepFlag = 0x02; + } - if (cfg->ptp_type == SYNC) - ptp_type = FOLLOW_UP; - else if (cfg->ptp_type == PDELAY_RESP) - ptp_type = PDELAY_RESP_FUP; + ptp_set_flags(hdr, twoStepFlag); +} + +int get_event_type(struct pkt_cfg *cfg, int type) +{ + if (cfg->listen == 1) // Force listen + return TRANS_EVENT; + if (cfg->listen == 0) // Force quiet + return TRANS_GENERAL; + + if (type == PDELAY_REQ) + return TRANS_EVENT; // Always timestamp pdelay_req + else if (cfg->tstype == TS_P2P1STEP && type == PDELAY_RESP) + return TRANS_GENERAL; // Don't timestamp pdelay_resp on p2p1step + else if ((cfg->tstype == TS_ONESTEP || cfg->tstype == TS_P2P1STEP) && type == SYNC) + return TRANS_GENERAL; // Don't timestamp sync on onestep and p2p1step + else if (type & 0x8) + return TRANS_GENERAL; // Don't timestamp general packets else - return -EINVAL; + return TRANS_EVENT; // Timestamp rest +} + +int build_and_send(struct pkt_cfg *cfg, int sock, int type, struct hw_timestamp *hwts) +{ + struct ptp_header hdr; + union Message tx_msg; + int event_type; + int err; hdr = ptp_header_template(); - ptp_set_type(&hdr, ptp_type); + ptp_set_type(&hdr, type); ptp_set_seqId(&hdr, cfg->seq); ptp_set_dmac(&hdr, cfg->mac); ptp_set_transport_specific(&hdr, cfg->transportSpecific); ptp_set_version(&hdr, cfg->version); ptp_set_domain(&hdr, cfg->domain); - ptp_set_flags(&hdr, 0); - msg = ptp_msg_create_type(hdr, ptp_type); - return raw_send(sock, TRANS_GENERAL, &msg, ptp_msg_get_size(ptp_type), NULL); + set_two_step_flag(cfg, &hdr, type); + + tx_msg = ptp_msg_create_type(hdr, type); + + event_type = get_event_type(cfg, type); + + err = raw_send(sock, event_type, &tx_msg, ptp_msg_get_size(type), hwts); + print_ts("TS: ", hwts->ts.ns); + hwts->ts.ns = 0; + return err; +} + +int send_auto_fup(struct pkt_cfg *cfg, int sock, int type, struct hw_timestamp *hwts) +{ + struct ptp_header hdr; + union Message msg; + int ptp_type; + int err; + + if (type == SYNC) + type = FOLLOW_UP; + else if (type == PDELAY_RESP) + type = PDELAY_RESP_FUP; + else + return -EINVAL; + + return build_and_send(cfg, sock, ptp_type, hwts); +} + +int tx_mode(struct pkt_cfg *cfg, int sock, struct hw_timestamp *hwts) +{ + int type; + int i; + + while (cfg->count || cfg->nonstop_flag) { + for (i = 0; i < cfg->sequence_length; i++) { + type = cfg->sequence_types[i]; + build_and_send(cfg, sock, type, hwts); + if (cfg->auto_fup && (type == SYNC || type == PDELAY_RESP)) { + /* Allow the sync to send first to avoid out-of-order */ + usleep(50000); + send_auto_fup(cfg, sock, type, hwts); + } + cfg->seq++; + } + // Not working + if (!cfg->nonstop_flag) + cfg->count--; + } +} + +int rx_mode(struct pkt_cfg *cfg, int sock, struct hw_timestamp *hwts) +{ + unsigned char buf[1600]; + union Message *rx_msg; + + rx_msg = (union Message *)buf; + + while (1) { + sk_receive(sock, rx_msg, 1600, NULL, hwts, 0); + printf("Type: %s. ", ptp_type2str(rx_msg->hdr.tsmt & 0xF)); + print_ts("TS: ", hwts->ts.ns); + } } int pkt_parse_opt(int argc, char **argv, struct pkt_cfg *cfg) { + int type; int c; str2mac("ff:ff:ff:ff:ff:ff", cfg->mac); @@ -119,6 +216,7 @@ int pkt_parse_opt(int argc, char **argv, struct pkt_cfg *cfg) cfg->version = 2 | (1 << 4); cfg->twoStepFlag = 1; cfg->count = 1; + cfg->listen = -1; struct option long_options[] = { { "help", no_argument, NULL, 'h' }, { "transportSpecific", required_argument, NULL, 1 }, @@ -130,8 +228,7 @@ int pkt_parse_opt(int argc, char **argv, struct pkt_cfg *cfg) return EINVAL; } - while ((c = getopt_long(argc, argv, "StrapdfDl:hoOi:m:c:s:T:v:", long_options, NULL)) != - -1) { + while ((c = getopt_long(argc, argv, "StrapdfD:l:hoOi:m:c:s:T:v:", long_options, NULL)) != -1) { switch (c) { case 1: cfg->transportSpecific = strtoul(optarg, NULL, 0); @@ -141,11 +238,19 @@ int pkt_parse_opt(int argc, char **argv, struct pkt_cfg *cfg) cfg->twoStepFlag_set = 1; break; case 'T': - cfg->ptp_type = str2ptp_type(optarg); - if (cfg->ptp_type < 0) { + if (cfg->sequence_length >= SEQUENCE_MAX) { + printf("Sequence too long. Max %d\n", SEQUENCE_MAX); + return -1; + } + + type = str2ptp_type(optarg); + if (type < 0) { printf("Invalid ptp type\n"); + printf("Expected: sync|follow_up|delay_req|delay_resp|pdelay_req|pdelay_resp|pdelay_resp_fup|announce|management|signaling\n"); return -1; } + cfg->sequence_types[cfg->sequence_length] = type; + cfg->sequence_length++; break; case 'S': cfg->tstype = TS_SOFTWARE; @@ -154,17 +259,14 @@ int pkt_parse_opt(int argc, char **argv, struct pkt_cfg *cfg) cfg->tstamp_all = 1; break; case 'o': - cfg->onestep = 1; cfg->tstype = TS_ONESTEP; break; case 'O': - cfg->onestep = 1; cfg->tstype = TS_P2P1STEP; break; case 'l': - cfg->tstype = TS_ONESTEP; - cfg->onestep_listen = 1; - cfg->onestep = 1; + // Force listen or not listen to TS + cfg->listen = strtol(optarg, NULL, 0); break; case 's': cfg->seq = strtoul(optarg, NULL, 0); @@ -184,9 +286,6 @@ int pkt_parse_opt(int argc, char **argv, struct pkt_cfg *cfg) case 'c': cfg->count = strtoul(optarg, NULL, 0); break; - case 'p': - cfg->prio = strtoul(optarg, NULL, 0); - break; case 'f': cfg->auto_fup = 1; break; @@ -223,6 +322,11 @@ int pkt_parse_opt(int argc, char **argv, struct pkt_cfg *cfg) } } + if (cfg->sequence_length == 0) { + cfg->sequence_types[0] = SYNC; + cfg->sequence_length = 1; + } + return 0; } @@ -231,48 +335,23 @@ int run_pkt_mode(int argc, char **argv) enum transport_event event_type; struct pkt_cfg cfg = { 0 }; struct hw_timestamp hwts; - unsigned char buf[1600]; - struct ptp_header hdr; - union Message tx_msg; - union Message *rx_msg; int sock; int err; err = pkt_parse_opt(argc, argv, &cfg); - - hdr = ptp_header_template(); - ptp_set_type(&hdr, cfg.ptp_type); - ptp_set_seqId(&hdr, cfg.seq); - ptp_set_dmac(&hdr, cfg.mac); - ptp_set_transport_specific(&hdr, cfg.transportSpecific); - ptp_set_version(&hdr, cfg.version); - ptp_set_domain(&hdr, cfg.domain); + if (err) + return err; if (!cfg.interface) { fprintf(stderr, "Error: missing input interface\n"); return EINVAL; } - if (cfg.auto_fup && (cfg.ptp_type != SYNC && cfg.ptp_type != PDELAY_RESP)) { - fprintf(stderr, - "Error: auto-follow-up can only be used with sync and pdelay_resp\n"); - return EINVAL; - } - - if (cfg.twoStepFlag_set) { - if (cfg.twoStepFlag) - ptp_set_flags(&hdr, 0x02); - else - ptp_set_flags(&hdr, 0x00); - } else { - /* Auto-clear twoStepFlag when one-step sync is set. - * Later this also needs to handle p2p1step. - */ - if (cfg.onestep && cfg.ptp_type == SYNC || cfg.tstype == TS_P2P1STEP && cfg.ptp_type == PDELAY_RESP) - ptp_set_flags(&hdr, 0); - } - - tx_msg = ptp_msg_create_type(hdr, cfg.ptp_type); + /*if (cfg.auto_fup && (cfg.ptp_type != SYNC && cfg.ptp_type != PDELAY_RESP)) {*/ + /*fprintf(stderr,*/ + /*"Error: auto-follow-up can only be used with sync and pdelay_resp\n");*/ + /*return EINVAL;*/ + /*}*/ if (!cfg.count) cfg.nonstop_flag = 1; @@ -281,8 +360,6 @@ int run_pkt_mode(int argc, char **argv) * pdelay_req/resp work correctly with one-step/p2p1step? */ - rx_msg = (union Message *)buf; - hwts.type = cfg.tstype; hwts.ts.ns = 0; sock = open_socket(cfg.interface, 1, ptp_dst_mac, p2p_dst_mac, 0, 1); @@ -297,32 +374,10 @@ int run_pkt_mode(int argc, char **argv) return err; } - if (cfg.rx_only) { - while (1) { - sk_receive(sock, rx_msg, 1600, NULL, &hwts, 0); - printf("Type: %s. ", ptp_type2str(rx_msg->hdr.tsmt & 0xF)); - print_ts("TS: ", hwts.ts.ns); - } - } - - if (cfg.onestep && !cfg.onestep_listen) - event_type = cfg.tstype == TS_P2P1STEP ? TRANS_P2P1STEP : TRANS_ONESTEP; - else if (cfg.ptp_type & 0x8) - event_type = TRANS_GENERAL; + if (cfg.rx_only) + rx_mode(&cfg, sock, &hwts); else - event_type = TRANS_EVENT; - - while (cfg.count || cfg.nonstop_flag) { - err = raw_send(sock, event_type, &tx_msg, ptp_msg_get_size(cfg.ptp_type), &hwts); - print_ts("TS: ", hwts.ts.ns); - if (!cfg.nonstop_flag) - cfg.count--; - if (cfg.auto_fup) { - /* Allow the sync to send first to avoid out-of-order */ - usleep(50000); - send_auto_fup(&cfg, sock); - } - } + tx_mode(&cfg, sock, &hwts); return 0; }