diff --git a/makefile b/makefile index faa9329..901e10a 100644 --- a/makefile +++ b/makefile @@ -24,7 +24,7 @@ CFLAGS = -Wall $(INC) $(DEBUG) LDFLAGS = LDLIBS = -lm -lrt PRG = linuxptp -OBJ = bmc.o fsm.o phc.o print.o transport.o udp.o util.o +OBJ = bmc.o fsm.o msg.o phc.o print.o transport.o udp.o util.o SRC = $(OBJ:.o=.c) DEPEND = $(OBJ:.o=.d) diff --git a/msg.c b/msg.c new file mode 100644 index 0000000..703e018 --- /dev/null +++ b/msg.c @@ -0,0 +1,257 @@ +/** + * @file msg.c + * @note Copyright (C) 2011 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 "msg.h" + +#define VERSION_MASK 0x0f +#define VERSION 0x02 + +static TAILQ_HEAD(msg_pool, ptp_message) msg_pool; + +static void announce_post_recv(struct announce_msg *m) +{ + m->currentUtcOffset = ntohs(m->currentUtcOffset); + m->grandmasterClockQuality.offsetScaledLogVariance = + ntohs(m->grandmasterClockQuality.offsetScaledLogVariance); + m->stepsRemoved = ntohs(m->stepsRemoved); +} + +static int64_t host2net64(int64_t val) +{ + return __cpu_to_be64(val); +} + +static int64_t net2host64(int64_t val) +{ + return __be64_to_cpu(val); +} + +static int hdr_post_recv(struct ptp_header *m) +{ + if ((m->ver & VERSION_MASK) != VERSION) + return -1; + m->messageLength = ntohs(m->messageLength); + m->correction = net2host64(m->correction); + m->sourcePortIdentity.portNumber = ntohs(m->sourcePortIdentity.portNumber); + m->sequenceId = ntohs(m->sequenceId); + return 0; +} + +static int hdr_pre_send(struct ptp_header *m) +{ + m->messageLength = htons(m->messageLength); + m->correction = host2net64(m->correction); + m->sourcePortIdentity.portNumber = htons(m->sourcePortIdentity.portNumber); + m->sequenceId = htons(m->sequenceId); + return 0; +} + +static char *msg_type_string(int type) +{ + switch (type) { + case SYNC: + return "SYNC"; + case DELAY_REQ: + return "DELAY_REQ"; + case PDELAY_REQ: + return "PDELAY_REQ"; + case PDELAY_RESP: + return "PDELAY_RESP"; + case FOLLOW_UP: + return "FOLLOW_UP"; + case DELAY_RESP: + return "DELAY_RESP"; + case PDELAY_RESP_FOLLOW_UP: + return "PDELAY_RESP_FOLLOW_UP"; + case ANNOUNCE: + return "ANNOUNCE"; + case SIGNALING: + return "SIGNALING"; + case MANAGEMENT: + return "MANAGEMENT"; + } + return "unknown"; +} + +static void timestamp_post_recv(struct ptp_message *m, struct Timestamp *ts) +{ + uint32_t lsb = ntohl(ts->seconds_lsb); + uint16_t msb = ntohs(ts->seconds_msb); + + m->ts.pdu.sec = ((uint64_t)lsb) | (((uint64_t)msb) << 32); + m->ts.pdu.nsec = ntohl(ts->nanoseconds); +} + +static void timestamp_pre_send(struct ptp_message *m, struct Timestamp *ts) +{ + ts->seconds_lsb = htonl(m->ts.pdu.sec); + ts->seconds_msb = htons(0); + ts->nanoseconds = htonl(m->ts.pdu.nsec); +} + +/* public methods */ + +struct ptp_message *msg_allocate(void) +{ + struct ptp_message *m = TAILQ_FIRST(&msg_pool); + if (!m) + m = malloc(sizeof(*m)); + if (m) + m->refcnt = 1; + return m; +} + +void msg_get(struct ptp_message *m) +{ + m->refcnt++; +} + +int msg_post_recv(struct ptp_message *m, int cnt) +{ + int type; + + if (cnt < sizeof(struct ptp_header)) + return -1; + + if (hdr_post_recv(&m->header)) + return -1; + + type = msg_type(m); + + switch (type) { + case SYNC: + if (cnt < sizeof(struct sync_msg)) + return -1; + timestamp_post_recv(m, &m->sync.originTimestamp); + break; + case DELAY_REQ: + if (cnt < sizeof(struct delay_req_msg)) + return -1; + break; + case PDELAY_REQ: + if (cnt < sizeof(struct pdelay_req_msg)) + return -1; + break; + case PDELAY_RESP: + if (cnt < sizeof(struct pdelay_resp_msg)) + return -1; + break; + case FOLLOW_UP: + if (cnt < sizeof(struct follow_up_msg)) + return -1; + timestamp_post_recv(m, &m->follow_up.preciseOriginTimestamp); + break; + case DELAY_RESP: + if (cnt < sizeof(struct delay_resp_msg)) + return -1; + timestamp_post_recv(m, &m->delay_resp.receiveTimestamp); + break; + case PDELAY_RESP_FOLLOW_UP: + if (cnt < sizeof(struct pdelay_resp_fup_msg)) + return -1; + break; + case ANNOUNCE: + if (cnt < sizeof(struct announce_msg)) + return -1; + clock_gettime(CLOCK_MONOTONIC, &m->ts.host); + timestamp_post_recv(m, &m->announce.originTimestamp); + announce_post_recv(&m->announce); + break; + case SIGNALING: + case MANAGEMENT: + default: + return -1; + } + return 0; +} + +int msg_pre_send(struct ptp_message *m) +{ + int type; + + if (hdr_pre_send(&m->header)) + return -1; + + type = msg_type(m); + + switch (type) { + case SYNC: + return -1; + case DELAY_REQ: + timestamp_pre_send(m, &m->delay_req.originTimestamp); + break; + case PDELAY_REQ: + case PDELAY_RESP: + case FOLLOW_UP: + case DELAY_RESP: + case PDELAY_RESP_FOLLOW_UP: + case ANNOUNCE: + case SIGNALING: + case MANAGEMENT: + default: + return -1; + } + return 0; +} + +void msg_print(struct ptp_message *m, FILE *fp) +{ + fprintf(fp, + "\t" + "%-10s " +// "versionPTP 0x%02X " +// "messageLength %hu " +// "domainNumber %u " +// "reserved1 0x%02X " +// "flagField 0x%02X%02X " +// "correction %lld " +// "reserved2 %u " +// "sourcePortIdentity ... " + "sequenceId %4hu " +// "control %u " +// "logMessageInterval %d " + , + msg_type_string(msg_type(m)), +// m->header.ver, +// m->header.messageLength, +// m->header.domainNumber, +// m->header.reserved1, +// m->header.flagField[0], +// m->header.flagField[1], +// m->header.correction, +// m->header.reserved2, +// m->header.sourcePortIdentity, + m->header.sequenceId +// m->header.control, +// m->header.logMessageInterval + ); + fprintf(fp, "\n"); +} + +void msg_put(struct ptp_message *m) +{ + m->refcnt--; + if (!m) + TAILQ_INSERT_HEAD(&msg_pool, m, list); +} diff --git a/msg.h b/msg.h new file mode 100644 index 0000000..fab5907 --- /dev/null +++ b/msg.h @@ -0,0 +1,249 @@ +/** + * @file msg.h + * @brief Implements the various PTP message types. + * @note Copyright (C) 2011 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. + */ +#ifndef HAVE_MSG_H +#define HAVE_MSG_H + +#include +#include +#include + +#include "ddt.h" +#include "transport.h" + +/* Values for the messageType field */ +#define SYNC 0x0 +#define DELAY_REQ 0x1 +#define PDELAY_REQ 0x2 +#define PDELAY_RESP 0x3 +#define FOLLOW_UP 0x8 +#define DELAY_RESP 0x9 +#define PDELAY_RESP_FOLLOW_UP 0xA +#define ANNOUNCE 0xB +#define SIGNALING 0xC +#define MANAGEMENT 0xD + +/* Bits for flagField[0] */ +#define ALT_MASTER (1<<0) +#define TWO_STEP (1<<1) +#define UNICAST (1<<2) + +/* Bits for flagField[1] */ +#define LEAP_61 (1<<0) +#define LEAP_59 (1<<1) +#define UTC_OFF_VALID (1<<2) +#define PTP_TIMESCALE (1<<3) +#define TIME_TRACEABLE (1<<4) +#define FREQ_TRACEABLE (1<<5) + +enum controlField { + CTL_SYNC, + CTL_DELAY_REQ, + CTL_FOLLOW_UP, + CTL_DELAY_RESP, + CTL_MANAGEMENT, + CTL_OTHER, +}; + +struct ptp_header { + uint8_t tsmt; /* transportSpecific | messageType */ + uint8_t ver; /* reserved | versionPTP */ + UInteger16 messageLength; + UInteger8 domainNumber; + Octet reserved1; + Octet flagField[2]; + Integer64 correction; + UInteger32 reserved2; + struct PortIdentity sourcePortIdentity; + UInteger16 sequenceId; + UInteger8 control; + Integer8 logMessageInterval; +} PACKED; + +struct announce_msg { + struct ptp_header hdr; + struct Timestamp originTimestamp; + Integer16 currentUtcOffset; + Octet reserved; + UInteger8 grandmasterPriority1; + struct ClockQuality grandmasterClockQuality; + UInteger8 grandmasterPriority2; + struct ClockIdentity grandmasterIdentity; + UInteger16 stepsRemoved; + Enumeration8 timeSource; +} PACKED; + +struct sync_msg { + struct ptp_header hdr; + struct Timestamp originTimestamp; +} PACKED; + +struct delay_req_msg { + struct ptp_header hdr; + struct Timestamp originTimestamp; +} PACKED; + +struct follow_up_msg { + struct ptp_header hdr; + struct Timestamp preciseOriginTimestamp; +} PACKED; + +struct delay_resp_msg { + struct ptp_header hdr; + struct Timestamp receiveTimestamp; + struct PortIdentity requestingPortIdentity; +} PACKED; + +struct pdelay_req_msg { + struct ptp_header hdr; + struct Timestamp originTimestamp; + struct PortIdentity reserved; +} PACKED; + +struct pdelay_resp_msg { + struct ptp_header hdr; + struct Timestamp requestReceiptTimestamp; + struct PortIdentity requestingPortIdentity; +} PACKED; + +struct pdelay_resp_fup_msg { + struct ptp_header hdr; + struct Timestamp responeOriginTimestamp; + struct PortIdentity requestingPortIdentity; +} PACKED; + +struct ptp_message { + union { + struct ptp_header header; + struct announce_msg announce; + struct sync_msg sync; + struct delay_req_msg delay_req; + struct follow_up_msg follow_up; + struct delay_resp_msg delay_resp; + struct pdelay_req_msg pdelay_req; + struct pdelay_resp_msg pdelay_resp; + struct pdelay_resp_fup_msg pdelay_resp_fup; + } PACKED; + /**/ + int refcnt; + TAILQ_ENTRY(ptp_message) list; + struct { + /** + * Contains the time stamp from the packet data in a + * native binary format for the host machine. The + * exact source of the time stamp's value depends on + * the message type: + * + * - announce originTimestamp + * - follow_up preciseOriginTimestamp + * - sync originTimestamp + * - delay_req originTimestamp + */ + struct timestamp pdu; + /** + * Approximate ingress time stamp using the relative + * CLOCK_MONOTONIC. Used to determine when announce + * messages have expired. + */ + struct timespec host; + } ts; + /** + * Contains the ingress time stamp obtained by the + * SO_TIMESTAMPING socket option. + */ + struct hw_timestamp hwts; +}; + +/** + * Test a given bit in a message's flag field. + * @param m Message to test. + * @param index Index into flag field, either 0 or 1. + * @param bit Bit mask of one bit to test. + * @return One if bit is set, zero otherwise. + */ +static inline Boolean field_is_set(struct ptp_message *m, int index, Octet bit) +{ + return m->header.flagField[index] & bit ? TRUE : FALSE; +} + +/** + * Obtain the message type. + * @param m Message to test. + * @return The value of the messageType field. + */ +static inline int msg_type(struct ptp_message *m) +{ + return m->header.tsmt & 0x0f; +} + +/** + * Allocate a new message instance. + * + * Messages are reference counted, and newly allocated messages have a + * reference count of one. Allocated messages are freed using the + * function @ref msg_put(). + * + * @return Pointer to a message on success, NULL otherwise. + */ +struct ptp_message *msg_allocate(void); + +/** + * Obtain a reference to a message, increasing its reference count by one. + * @param m A message obtained using @ref msg_allocate(). + */ +void msg_get(struct ptp_message *m); + +/** + * Process messages after reception. + * @param m A message obtained using @ref msg_allocate(). + * @return Zero on success, non-zero if the message is invalid. + */ +int msg_post_recv(struct ptp_message *m, int cnt); + +/** + * Prepare messages for transmission. + * @param m A message obtained using @ref msg_allocate(). + * @return Zero on success, non-zero if the message is invalid. + */ +int msg_pre_send(struct ptp_message *m); + +/** + * Print messages for debugging purposes. + * @param m A message obtained using @ref msg_allocate(). + * @param fp An open file pointer. + */ +void msg_print(struct ptp_message *m, FILE *fp); + +/** + * Release a reference to a message, decreasing its reference count by one. + * @param m A message obtained using @ref msg_allocate(). + */ +void msg_put(struct ptp_message *m); + +/** + * Test whether a message is one-step message. + * @param m Message to test. + * @return One if the message is a one-step, zero otherwise. + */ +static inline Boolean one_step(struct ptp_message *m) +{ + return !field_is_set(m, 0, TWO_STEP); +} + +#endif