diff --git a/makefile b/makefile index 2587dce..b5301ad 100644 --- a/makefile +++ b/makefile @@ -25,7 +25,7 @@ LDFLAGS = LDLIBS = -lm -lrt PRG = ptp4l phc2sys hwstamp_ctl OBJ = bmc.o clock.o config.o fsm.o ptp4l.o mave.o msg.o phc.o pi.o port.o \ - print.o servo.o tmtab.o transport.o udp.o util.o + print.o servo.o sk.o tmtab.o transport.o udp.o util.o OBJECTS = $(OBJ) phc2sys.o hwstamp_ctl.o SRC = $(OBJECTS:.o=.c) diff --git a/ptp4l.c b/ptp4l.c index f7fa0f6..845f09a 100644 --- a/ptp4l.c +++ b/ptp4l.c @@ -24,8 +24,8 @@ #include "clock.h" #include "config.h" #include "print.h" +#include "sk.h" #include "transport.h" -#include "udp.h" #define DEFAULT_PHC "/dev/ptp0" @@ -36,7 +36,7 @@ static struct port_defaults pod; static int generate_clock_identity(struct ClockIdentity *ci, char *name) { unsigned char mac[6]; - if (udp_interface_macaddr(name, mac, sizeof(mac))) + if (sk_interface_macaddr(name, mac, sizeof(mac))) return -1; ci->id[0] = mac[0]; ci->id[1] = mac[1]; diff --git a/sk.c b/sk.c new file mode 100644 index 0000000..ed8065b --- /dev/null +++ b/sk.c @@ -0,0 +1,215 @@ +/** + * @file sk.c + * @brief Implements protocol independent socket methods. + * @note Copyright (C) 2012 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 "print.h" +#include "sk.h" + +/* private methods */ + +static int hwts_init(int fd, char *device) +{ + struct ifreq ifreq; + struct hwtstamp_config cfg, req; + int err; + + memset(&ifreq, 0, sizeof(ifreq)); + memset(&cfg, 0, sizeof(cfg)); + + strncpy(ifreq.ifr_name, device, sizeof(ifreq.ifr_name)); + + ifreq.ifr_data = (void *) &cfg; + cfg.tx_type = HWTSTAMP_TX_ON; + cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; + + req = cfg; + err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); + if (err < 0) + pr_err("ioctl SIOCSHWTSTAMP failed: %m"); + + if (memcmp(&cfg, &req, sizeof(cfg))) { + + pr_warning("driver changed our HWTSTAMP options"); + pr_warning("tx_type %d not %d", cfg.tx_type, req.tx_type); + pr_warning("rx_filter %d not %d", cfg.rx_filter, req.rx_filter); + + if (cfg.tx_type != HWTSTAMP_TX_ON || + cfg.rx_filter != HWTSTAMP_FILTER_ALL) { + return -1; + } + } + + return err ? errno : 0; +} + +/* public methods */ + +int sk_interface_index(int fd, char *name) +{ + struct ifreq ifreq; + int err; + + memset(&ifreq, 0, sizeof(ifreq)); + strcpy(ifreq.ifr_name, name); + err = ioctl(fd, SIOCGIFINDEX, &ifreq); + if (err < 0) { + pr_err("ioctl SIOCGIFINDEX failed: %m"); + return err; + } + return ifreq.ifr_ifindex; +} + +int sk_interface_macaddr(char *name, unsigned char *mac, int len) +{ + struct ifreq ifreq; + int err, fd; + + memset(&ifreq, 0, sizeof(ifreq)); + strcpy(ifreq.ifr_name, name); + + fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (fd < 0) { + pr_err("socket failed: %m"); + return -1; + } + + err = ioctl(fd, SIOCGIFHWADDR, &ifreq); + if (err < 0) { + pr_err("ioctl SIOCGIFHWADDR failed: %m"); + close(fd); + return -1; + } + + memcpy(mac, ifreq.ifr_hwaddr.sa_data, len); + close(fd); + return 0; +} + +int sk_receive(int fd, void *buf, int buflen, + struct hw_timestamp *hwts, int flags) +{ + char control[256]; + int cnt, level, try_again, type; + struct cmsghdr *cm; + struct iovec iov = { buf, buflen }; + struct msghdr msg; + struct timespec *ts = NULL; + + memset(control, 0, sizeof(control)); + memset(&msg, 0, sizeof(msg)); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + + try_again = flags == MSG_ERRQUEUE ? 2 : 1; + + for ( ; try_again; try_again--) { + cnt = recvmsg(fd, &msg, flags); + if (cnt >= 0) { + break; + } + if (errno == EINTR) { + try_again++; + } else if (errno == EAGAIN) { + usleep(1); + } else { + if (flags == MSG_ERRQUEUE) + pr_err("recvmsg tx timestamp failed: %m"); + else + pr_err("recvmsg failed: %m"); + break; + } + } + + for (cm = CMSG_FIRSTHDR(&msg); cm != NULL; cm = CMSG_NXTHDR(&msg, cm)) { + level = cm->cmsg_level; + type = cm->cmsg_type; + if (SOL_SOCKET == level && SO_TIMESTAMPING == type) { + if (cm->cmsg_len < sizeof(*ts) * 3) { + pr_warning("short SO_TIMESTAMPING message"); + return -1; + } + ts = (struct timespec *) CMSG_DATA(cm); + break; + } + } + + if (!ts) { + memset(&hwts->ts, 0, sizeof(hwts->ts)); + return cnt; + } + + switch (hwts->type) { + case TS_SOFTWARE: + hwts->ts = ts[0]; + break; + case TS_HARDWARE: + hwts->ts = ts[2]; + break; + case TS_LEGACY_HW: + hwts->ts = ts[1]; + break; + } + return cnt; +} + +int sk_timestamping_init(int fd, char *device, enum timestamp_type type) +{ + int flags; + + switch (type) { + case TS_SOFTWARE: + flags = SOF_TIMESTAMPING_TX_SOFTWARE | + SOF_TIMESTAMPING_RX_SOFTWARE | + SOF_TIMESTAMPING_SOFTWARE; + break; + case TS_HARDWARE: + flags = SOF_TIMESTAMPING_TX_HARDWARE | + SOF_TIMESTAMPING_RX_HARDWARE | + SOF_TIMESTAMPING_RAW_HARDWARE; + break; + case TS_LEGACY_HW: + flags = SOF_TIMESTAMPING_TX_HARDWARE | + SOF_TIMESTAMPING_RX_HARDWARE | + SOF_TIMESTAMPING_SYS_HARDWARE; + break; + default: + return -1; + } + + if (type != TS_SOFTWARE && hwts_init(fd, device)) + return -1; + + if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, + &flags, sizeof(flags)) < 0) { + pr_err("ioctl SO_TIMESTAMPING failed: %m"); + return -1; + } + + return 0; +} diff --git a/sk.h b/sk.h new file mode 100644 index 0000000..cfe49fd --- /dev/null +++ b/sk.h @@ -0,0 +1,63 @@ +/** + * @file sk.h + * @brief Implements protocol independent socket methods. + * @note Copyright (C) 2012 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_SK_H +#define HAVE_SK_H + +#include "transport.h" + +/** + * Obtain the numerical index from a network interface by name. + * @param fd An open socket. + * @param device The name of the network interface of interest. + * @return The result from the SIOCGIFINDEX ioctl. + */ +int sk_interface_index(int fd, char *device); + +/** + * Obtain the MAC address of a network interface. + * @param name The name of the interface + * @param mac Buffer to hold the result + * @param len Length of 'mac' + * @return Zero on success, non-zero otherwise. + */ +int sk_interface_macaddr(char *name, unsigned char *mac, int len); + +/** + * Read a message from a socket. + * @param fd An open socket. + * @param buf Buffer to receive the message. + * @param buflen Size of 'buf' in bytes. + * @param hwts Pointer to a buffer to receive the message's time stamp. + * @param flags Flags to pass to RECV(2). + * @return + */ +int sk_receive(int fd, void *buf, int buflen, + struct hw_timestamp *hwts, int flags); + +/** + * Enable time stamping on a given network interface. + * @param fd An open socket. + * @param device The name of the network interface to configure. + * @param type The requested flavor of time stamping. + * @return Zero on success, non-zero otherwise. + */ +int sk_timestamping_init(int fd, char *device, enum timestamp_type type); + +#endif diff --git a/udp.c b/udp.c index fc67175..13bcd1e 100644 --- a/udp.c +++ b/udp.c @@ -28,11 +28,8 @@ #include #include -#include -#include -#include - #include "print.h" +#include "sk.h" #include "transport_private.h" #include "udp.h" @@ -40,92 +37,6 @@ #define GENERAL_PORT 320 #define MULTICAST_IP_ADDR "224.0.1.129" -static int hwts_init(int fd, char *device) -{ - struct ifreq ifreq; - struct hwtstamp_config cfg, req; - int err; - - memset(&ifreq, 0, sizeof(ifreq)); - memset(&cfg, 0, sizeof(cfg)); - - strncpy(ifreq.ifr_name, device, sizeof(ifreq.ifr_name)); - - ifreq.ifr_data = (void *) &cfg; - cfg.tx_type = HWTSTAMP_TX_ON; - cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; - - req = cfg; - err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); - if (err < 0) - pr_err("ioctl SIOCSHWTSTAMP failed: %m"); - - if (memcmp(&cfg, &req, sizeof(cfg))) { - - pr_warning("driver changed our HWTSTAMP options"); - pr_warning("tx_type %d not %d", cfg.tx_type, req.tx_type); - pr_warning("rx_filter %d not %d", cfg.rx_filter, req.rx_filter); - - if (cfg.tx_type != HWTSTAMP_TX_ON || - cfg.rx_filter != HWTSTAMP_FILTER_ALL) { - return -1; - } - } - - return err ? errno : 0; -} - -static int timestamping_init(int fd, char *device, enum timestamp_type type) -{ - int flags; - - switch (type) { - case TS_SOFTWARE: - flags = SOF_TIMESTAMPING_TX_SOFTWARE | - SOF_TIMESTAMPING_RX_SOFTWARE | - SOF_TIMESTAMPING_SOFTWARE; - break; - case TS_HARDWARE: - flags = SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_RAW_HARDWARE; - break; - case TS_LEGACY_HW: - flags = SOF_TIMESTAMPING_TX_HARDWARE | - SOF_TIMESTAMPING_RX_HARDWARE | - SOF_TIMESTAMPING_SYS_HARDWARE; - break; - default: - return -1; - } - - if (type != TS_SOFTWARE && hwts_init(fd, device)) - return -1; - - if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, - &flags, sizeof(flags)) < 0) { - pr_err("ioctl SO_TIMESTAMPING failed: %m"); - return -1; - } - - return 0; -} - -static int interface_index(int fd, char *name) -{ - struct ifreq ifreq; - int err; - - memset(&ifreq, 0, sizeof(ifreq)); - strcpy(ifreq.ifr_name, name); - err = ioctl(fd, SIOCGIFINDEX, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGIFINDEX failed: %m"); - return err; - } - return ifreq.ifr_ifindex; -} - static int mcast_bind(int fd, int index) { int err; @@ -184,7 +95,7 @@ static int open_socket(char *name, struct in_addr *mc_addr, short port) pr_err("socket failed: %m"); goto no_socket; } - index = interface_index(fd, name); + index = sk_interface_index(fd, name); if (index < 0) goto no_option; @@ -233,7 +144,7 @@ static int udp_open(struct transport *t, char *name, struct fdarray *fda, if (gfd < 0) goto no_general; - if (timestamping_init(efd, name, ts_type)) + if (sk_timestamping_init(efd, name, ts_type)) goto no_timestamping; fda->fd[FD_EVENT] = efd; @@ -249,79 +160,10 @@ no_event: return -1; } -static int receive(int fd, void *buf, int buflen, - struct hw_timestamp *hwts, int flags) -{ - char control[256]; - int cnt, level, try_again, type; - struct cmsghdr *cm; - struct iovec iov = { buf, buflen }; - struct msghdr msg; - struct timespec *ts = NULL; - - memset(control, 0, sizeof(control)); - memset(&msg, 0, sizeof(msg)); - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - msg.msg_control = control; - msg.msg_controllen = sizeof(control); - - try_again = flags == MSG_ERRQUEUE ? 2 : 1; - - for ( ; try_again; try_again--) { - cnt = recvmsg(fd, &msg, flags); - if (cnt >= 0) { - break; - } - if (errno == EINTR) { - try_again++; - } else if (errno == EAGAIN) { - usleep(1); - } else { - if (flags == MSG_ERRQUEUE) - pr_err("recvmsg tx timestamp failed: %m"); - else - pr_err("recvmsg failed: %m"); - break; - } - } - - for (cm = CMSG_FIRSTHDR(&msg); cm != NULL; cm = CMSG_NXTHDR(&msg, cm)) { - level = cm->cmsg_level; - type = cm->cmsg_type; - if (SOL_SOCKET == level && SO_TIMESTAMPING == type) { - if (cm->cmsg_len < sizeof(*ts) * 3) { - pr_warning("short SO_TIMESTAMPING message"); - return -1; - } - ts = (struct timespec *) CMSG_DATA(cm); - break; - } - } - - if (!ts) { - memset(&hwts->ts, 0, sizeof(hwts->ts)); - return cnt; - } - - switch (hwts->type) { - case TS_SOFTWARE: - hwts->ts = ts[0]; - break; - case TS_HARDWARE: - hwts->ts = ts[2]; - break; - case TS_LEGACY_HW: - hwts->ts = ts[1]; - break; - } - return cnt; -} - static int udp_recv(struct transport *t, int fd, void *buf, int buflen, struct hw_timestamp *hwts) { - return receive(fd, buf, buflen, hwts, 0); + return sk_receive(fd, buf, buflen, hwts, 0); } static int udp_send(struct transport *t, struct fdarray *fda, int event, @@ -344,7 +186,7 @@ static int udp_send(struct transport *t, struct fdarray *fda, int event, /* * Get the time stamp right away. */ - return event ? receive(fd, junk, len, hwts, MSG_ERRQUEUE) : cnt; + return event ? sk_receive(fd, junk, len, hwts, MSG_ERRQUEUE) : cnt; } static struct transport the_udp_transport = { @@ -364,29 +206,3 @@ void udp_transport_destroy(struct transport *t) { /* No need for any per-instance deallocation. */ } - -int udp_interface_macaddr(char *name, unsigned char *mac, int len) -{ - struct ifreq ifreq; - int err, fd; - - memset(&ifreq, 0, sizeof(ifreq)); - strcpy(ifreq.ifr_name, name); - - fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (fd < 0) { - pr_err("socket failed: %m"); - return -1; - } - - err = ioctl(fd, SIOCGIFHWADDR, &ifreq); - if (err < 0) { - pr_err("ioctl SIOCGIFHWADDR failed: %m"); - close(fd); - return -1; - } - - memcpy(mac, ifreq.ifr_hwaddr.sa_data, len); - close(fd); - return 0; -} diff --git a/udp.h b/udp.h index 6506444..c93943a 100644 --- a/udp.h +++ b/udp.h @@ -23,15 +23,6 @@ #include "fd.h" #include "transport.h" -/** - * Obtain the MAC address of a network interface. - * @param name The name of the interface - * @param mac Buffer to hold the result - * @param len Length of 'mac' - * @return Zero on success, non-zero otherwise. - */ -int udp_interface_macaddr(char *name, unsigned char *mac, int len); - /** * Allocate an instance of a UDP/IPv4 transport. * @return Pointer to a new transport instance on success, NULL otherwise.