diff --git a/makefile b/makefile index d144949..2fa6a09 100644 --- a/makefile +++ b/makefile @@ -33,7 +33,7 @@ OBJ = bmc.o clock.o config.o fsm.o ptp4l.o mave.o msg.o phc.o pi.o port.o \ print.o raw.o servo.o sk.o tlv.o tmtab.o transport.o udp.o udp6.o uds.o util.o \ version.o -OBJECTS = $(OBJ) pmc.o phc2sys.o hwstamp_ctl.o sysoff.o +OBJECTS = $(OBJ) hwstamp_ctl.o phc2sys.o pmc.o pmc_common.o sysoff.o SRC = $(OBJECTS:.o=.c) DEPEND = $(OBJECTS:.o=.d) srcdir := $(dir $(lastword $(MAKEFILE_LIST))) @@ -49,7 +49,8 @@ all: $(PRG) ptp4l: $(OBJ) -pmc: pmc.o msg.o print.o raw.o sk.o tlv.o transport.o udp.o udp6.o uds.o util.o version.o +pmc: 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 phc2sys: phc2sys.o pi.o servo.o sk.o sysoff.o print.o version.o diff --git a/pmc.c b/pmc.c index 3d8ed68..3f94985 100644 --- a/pmc.c +++ b/pmc.c @@ -27,10 +27,9 @@ #include "ds.h" #include "fsm.h" -#include "msg.h" +#include "pmc_common.h" #include "print.h" #include "tlv.h" -#include "transport.h" #include "util.h" #include "version.h" @@ -40,14 +39,7 @@ #define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) #define P41 ((double)(1ULL << 41)) -static UInteger16 sequence_id; -static UInteger8 boundary_hops = 1; -static UInteger8 domain_number; -static UInteger8 transport_specific; -static struct PortIdentity port_identity; - -static struct transport *transport; -static struct fdarray fdarray; +static struct pmc *pmc; static void do_get_action(int action, int index); static void not_supported(int action, int index); @@ -112,52 +104,6 @@ struct management_id idtab[] = { { "LOG_MIN_PDELAY_REQ_INTERVAL", LOG_MIN_PDELAY_REQ_INTERVAL, not_supported }, }; -static struct ptp_message *pmc_message(uint8_t action) -{ - struct ptp_message *msg; - int pdulen; - - msg = msg_allocate(); - if (!msg) - return NULL; - - pdulen = sizeof(struct management_msg); - msg->hwts.type = TS_SOFTWARE; - - msg->header.tsmt = MANAGEMENT | transport_specific; - msg->header.ver = PTP_VERSION; - msg->header.messageLength = pdulen; - msg->header.domainNumber = domain_number; - msg->header.sourcePortIdentity = port_identity; - msg->header.sequenceId = sequence_id++; - msg->header.control = CTL_MANAGEMENT; - msg->header.logMessageInterval = 0x7f; - - memset(&msg->management.targetPortIdentity, 0xff, - sizeof(msg->management.targetPortIdentity)); - msg->management.startingBoundaryHops = boundary_hops; - msg->management.boundaryHops = boundary_hops; - msg->management.flags = action; - - return msg; -} - -static int pmc_send(struct ptp_message *msg, int pdulen) -{ - int cnt, err; - err = msg_pre_send(msg); - if (err) { - fprintf(stderr, "msg_pre_send failed\n"); - return -1; - } - cnt = transport_send(transport, &fdarray, 0, msg, pdulen, &msg->hwts); - if (cnt < 0) { - fprintf(stderr, "failed to send message\n"); - return -1; - } - return 0; -} - static char *action_string[] = { "GET", "SET", @@ -330,30 +276,10 @@ out: fflush(fp); } -static void get_action(int id) -{ - int pdulen; - struct ptp_message *msg; - struct management_tlv *mgt; - msg = pmc_message(GET); - if (!msg) { - return; - } - mgt = (struct management_tlv *) msg->management.suffix; - mgt->type = TLV_MANAGEMENT; - mgt->length = 2; - mgt->id = id; - pdulen = msg->header.messageLength + sizeof(*mgt); - msg->header.messageLength = pdulen; - msg->tlv_count = 1; - pmc_send(msg, pdulen); - msg_put(msg); -} - static void do_get_action(int action, int index) { if (action == GET) - get_action(idtab[index].code); + pmc_send_get_action(pmc, idtab[index].code); else fprintf(stderr, "%s only allows GET\n", idtab[index].name); } @@ -366,7 +292,7 @@ static void not_supported(int action, int index) static void null_management(int action, int index) { if (action == GET) - get_action(idtab[index].code); + pmc_send_get_action(pmc, idtab[index].code); else puts("non-get actions still todo"); } @@ -479,6 +405,7 @@ int main(int argc, char *argv[]) int c, cnt, length, tmo = -1; char line[1024]; enum transport_type transport_type = TRANS_UDP_IPV4; + UInteger8 boundary_hops = 1, domain_number = 0, transport_specific = 0; struct ptp_message *msg; #define N_FD 2 struct pollfd pollfd[N_FD]; @@ -531,32 +458,22 @@ int main(int argc, char *argv[]) if (!iface_name) { iface_name = transport_type == TRANS_UDS ? "/tmp/pmc" : "eth0"; } - if (transport_type != TRANS_UDS && - generate_clock_identity(&port_identity.clockIdentity, iface_name)) { - fprintf(stderr, "failed to generate a clock identity\n"); - return -1; - } - port_identity.portNumber = 1; - transport = transport_create(transport_type); - if (!transport) { - fprintf(stderr, "failed to create transport\n"); - return -1; - } - if (transport_open(transport, iface_name, &fdarray, TS_SOFTWARE)) { - fprintf(stderr, "failed to open transport\n"); - transport_destroy(transport); + + print_set_progname(progname); + print_set_syslog(1); + print_set_verbose(1); + + pmc = pmc_create(transport_type, iface_name, boundary_hops, domain_number, transport_specific); + if (!pmc) { + fprintf(stderr, "failed to create pmc\n"); return -1; } pollfd[0].fd = STDIN_FILENO; pollfd[0].events = POLLIN|POLLPRI; - pollfd[1].fd = fdarray.fd[FD_GENERAL]; + pollfd[1].fd = pmc_get_transport_fd(pmc); pollfd[1].events = POLLIN|POLLPRI; - print_set_progname(progname); - print_set_syslog(1); - print_set_verbose(1); - while (1) { cnt = poll(pollfd, N_FD, tmo); if (cnt < 0) { @@ -593,26 +510,14 @@ int main(int argc, char *argv[]) } } if (pollfd[1].revents & (POLLIN|POLLPRI)) { - msg = msg_allocate(); - if (!msg) { - fprintf(stderr, "low memory\n"); - return -1; - } - msg->hwts.type = TS_SOFTWARE; - cnt = transport_recv(transport, pollfd[1].fd, msg, - sizeof(msg->data), &msg->hwts); - if (cnt <= 0) { - fprintf(stderr, "recv message failed\n"); - } else if (msg_post_recv(msg, cnt)) { - fprintf(stderr, "bad message\n"); - } else { + msg = pmc_recv(pmc); + if (msg) { pmc_show(msg, stdout); + msg_put(msg); } - msg_put(msg); } } - transport_close(transport, &fdarray); - transport_destroy(transport); + pmc_destroy(pmc); return 0; } diff --git a/pmc_common.c b/pmc_common.c new file mode 100644 index 0000000..71854c0 --- /dev/null +++ b/pmc_common.c @@ -0,0 +1,187 @@ +/** + * @file pmc_common.c + * @note Copyright (C) 2012 Richard Cochran + * @note Copyright (C) 2013 Miroslav Lichvar + * + * 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 "print.h" +#include "tlv.h" +#include "transport.h" +#include "util.h" + +struct pmc { + UInteger16 sequence_id; + UInteger8 boundary_hops; + UInteger8 domain_number; + UInteger8 transport_specific; + struct PortIdentity port_identity; + + struct transport *transport; + struct fdarray fdarray; +}; + +struct pmc *pmc_create(enum transport_type transport_type, char *iface_name, + UInteger8 boundary_hops, UInteger8 domain_number, + UInteger8 transport_specific) +{ + struct pmc *pmc; + + pmc = calloc(1, sizeof *pmc); + if (!pmc) + return NULL; + + if (transport_type != TRANS_UDS && + generate_clock_identity(&pmc->port_identity.clockIdentity, + iface_name)) { + pr_err("failed to generate a clock identity"); + goto failed; + } + + pmc->port_identity.portNumber = 1; + pmc->boundary_hops = boundary_hops; + pmc->domain_number = domain_number; + pmc->transport_specific = transport_specific; + + pmc->transport = transport_create(transport_type); + if (!pmc->transport) { + pr_err("failed to create transport"); + goto failed; + } + if (transport_open(pmc->transport, iface_name, + &pmc->fdarray, TS_SOFTWARE)) { + pr_err("failed to open transport"); + goto failed; + } + + return pmc; + +failed: + if (pmc->transport) + transport_destroy(pmc->transport); + free(pmc); + return NULL; +} + +void pmc_destroy(struct pmc *pmc) +{ + transport_close(pmc->transport, &pmc->fdarray); + transport_destroy(pmc->transport); + free(pmc); +} + +static struct ptp_message *pmc_message(struct pmc *pmc, uint8_t action) +{ + struct ptp_message *msg; + int pdulen; + + msg = msg_allocate(); + if (!msg) + return NULL; + + pdulen = sizeof(struct management_msg); + msg->hwts.type = TS_SOFTWARE; + + msg->header.tsmt = MANAGEMENT | pmc->transport_specific; + msg->header.ver = PTP_VERSION; + msg->header.messageLength = pdulen; + msg->header.domainNumber = pmc->domain_number; + msg->header.sourcePortIdentity = pmc->port_identity; + msg->header.sequenceId = pmc->sequence_id++; + msg->header.control = CTL_MANAGEMENT; + msg->header.logMessageInterval = 0x7f; + + memset(&msg->management.targetPortIdentity, 0xff, + sizeof(msg->management.targetPortIdentity)); + msg->management.startingBoundaryHops = pmc->boundary_hops; + msg->management.boundaryHops = pmc->boundary_hops; + msg->management.flags = action; + + return msg; +} + +static int pmc_send(struct pmc *pmc, struct ptp_message *msg, int pdulen) +{ + int cnt, err; + err = msg_pre_send(msg); + if (err) { + pr_err("msg_pre_send failed"); + return -1; + } + cnt = transport_send(pmc->transport, &pmc->fdarray, 0, + msg, pdulen, &msg->hwts); + if (cnt < 0) { + pr_err("failed to send message"); + return -1; + } + return 0; +} + +int pmc_get_transport_fd(struct pmc *pmc) +{ + return pmc->fdarray.fd[FD_GENERAL]; +} + +int pmc_send_get_action(struct pmc *pmc, int id) +{ + int pdulen; + struct ptp_message *msg; + struct management_tlv *mgt; + msg = pmc_message(pmc, GET); + if (!msg) { + return -1; + } + mgt = (struct management_tlv *) msg->management.suffix; + mgt->type = TLV_MANAGEMENT; + mgt->length = 2; + mgt->id = id; + pdulen = msg->header.messageLength + sizeof(*mgt); + msg->header.messageLength = pdulen; + msg->tlv_count = 1; + pmc_send(pmc, msg, pdulen); + msg_put(msg); + + return 0; +} + +struct ptp_message *pmc_recv(struct pmc *pmc) +{ + struct ptp_message *msg; + int cnt; + + 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, sizeof(msg->data), &msg->hwts); + if (cnt <= 0) { + pr_err("recv message failed"); + goto failed; + } else if (msg_post_recv(msg, cnt)) { + pr_err("bad message"); + goto failed; + } + + return msg; +failed: + msg_put(msg); + return NULL; +} diff --git a/pmc_common.h b/pmc_common.h new file mode 100644 index 0000000..cd4d2c0 --- /dev/null +++ b/pmc_common.h @@ -0,0 +1,40 @@ +/** + * @file pmc_common.h + * @brief Code shared between PTP management clients. + * @note Copyright (C) 2013 Miroslav Lichvar + * + * 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_PMC_COMMON_H +#define HAVE_PMC_COMMON_H + +#include "msg.h" + +struct pmc; + +struct pmc *pmc_create(enum transport_type transport_type, char *iface_name, + UInteger8 boundary_hops, UInteger8 domain_number, + UInteger8 transport_specific); + +void pmc_destroy(struct pmc *pmc); + +int pmc_get_transport_fd(struct pmc *pmc); + +void pmc_send_get_action(struct pmc *pmc, int id); + +struct ptp_message *pmc_recv(struct pmc *pmc); + +#endif