From 60f3a85463d8b57725a8ca97afd5ee751dabba50 Mon Sep 17 00:00:00 2001 From: Richard Cochran Date: Sun, 6 Nov 2011 08:16:19 +0100 Subject: [PATCH] Add transport over UDP IPv4. Signed-off-by: Richard Cochran --- fd.h | 36 ++++++ makefile | 2 +- transport.c | 43 +++++++ transport.h | 62 +++++++++++ udp.c | 316 ++++++++++++++++++++++++++++++++++++++++++++++++++++ udp.h | 41 +++++++ 6 files changed, 499 insertions(+), 1 deletion(-) create mode 100644 fd.h create mode 100644 transport.c create mode 100644 transport.h create mode 100644 udp.c create mode 100644 udp.h diff --git a/fd.h b/fd.h new file mode 100644 index 0000000..88d3c36 --- /dev/null +++ b/fd.h @@ -0,0 +1,36 @@ +/** + * @file fd.h + * @brief Defines a array of file descriptors, useful for polling. + * @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_FD_H +#define HAVE_FD_H + +enum { + FD_EVENT, + FD_GENERAL, + FD_ANNOUNCE_TIMER, + FD_DELAY_TIMER, + N_POLLFD, +}; + +struct fdarray { + int fd[N_POLLFD]; + int cnt; +}; + +#endif diff --git a/makefile b/makefile index 21ec67a..faa9329 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 util.o +OBJ = bmc.o fsm.o phc.o print.o transport.o udp.o util.o SRC = $(OBJ:.o=.c) DEPEND = $(OBJ:.o=.d) diff --git a/transport.c b/transport.c new file mode 100644 index 0000000..0947037 --- /dev/null +++ b/transport.c @@ -0,0 +1,43 @@ +/** + * @file transport.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 "transport.h" +#include "udp.h" + +static struct transport udp = { + .close = udp_close, + .open = udp_open, + .recv = udp_recv, + .send = udp_send, +}; + +struct transport *transport_find(enum transport_type type) +{ + switch (type) { + case TRANS_UDP_IPV4: + return &udp; + case TRANS_UDP_IPV6: + case TRANS_IEEE_802_3: + case TRANS_DEVICENET: + case TRANS_CONTROLNET: + case TRANS_PROFINET: + break; + } + return NULL; +} diff --git a/transport.h b/transport.h new file mode 100644 index 0000000..4e9cd70 --- /dev/null +++ b/transport.h @@ -0,0 +1,62 @@ +/** + * @file transport.h + * @brief Defines an abstract transport layer. + * @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_TRANSPORT_H +#define HAVE_TRANSPORT_H + +#include + +#include "fd.h" + +enum transport_type { + TRANS_UDP_IPV4, + TRANS_UDP_IPV6, + TRANS_IEEE_802_3, + TRANS_DEVICENET, + TRANS_CONTROLNET, + TRANS_PROFINET, +}; + +enum timestamp_type { + TS_SOFTWARE, + TS_HARDWARE, + TS_LEGACY_HW, +}; + +struct hw_timestamp { + enum timestamp_type type; + struct timespec ts; +}; + +struct transport { + int (*close)(struct fdarray *fda); + int (*open)(char *name, struct fdarray *fda, enum timestamp_type tt); + int (*recv)(int fd, void *buf, int buflen, struct hw_timestamp *hwts); + int (*send)(struct fdarray *fda, int event, + void *buf, int buflen, struct hw_timestamp *hwts); +}; + +/** + * Obtain a pointer to the specified transport. + * @param type Which transport to obtain. + * @return Pointer to a static global. + */ +struct transport *transport_find(enum transport_type type); + +#endif diff --git a/udp.c b/udp.c new file mode 100644 index 0000000..b058a7f --- /dev/null +++ b/udp.c @@ -0,0 +1,316 @@ +/** + * @file udp.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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "udp.h" + +#define EVENT_PORT 319 +#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; + 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; + + err = ioctl(fd, SIOCSHWTSTAMP, &ifreq); + if (err < 0) + perror("SIOCSHWTSTAMP failed"); + + printf("tx_type %d\n" "rx_filter %d\n", cfg.tx_type, cfg.rx_filter); + + 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) { + perror("SO_TIMESTAMPING"); + 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) { + perror("ioctl SIOCGIFINDEX"); + return err; + } + return ifreq.ifr_ifindex; +} + +static int mcast_join(int fd, int index, const struct sockaddr *grp, + socklen_t grplen) +{ + int err; + struct group_req req; + req.gr_interface = index; + if (grplen > sizeof(req.gr_group)) { + return -1; + } + memcpy(&req.gr_group, grp, grplen); + err = setsockopt(fd, IPPROTO_IP, MCAST_JOIN_GROUP, &req, sizeof(req)); + if (err) { + perror("setsockopt MCAST_JOIN_GROUP"); + return -1; + } + return 0; +} + +int udp_close(struct fdarray *fda) +{ + close(fda->fd[0]); + close(fda->fd[1]); + return 0; +} + +static int open_socket(char *name, struct in_addr *mc_addr, short port) +{ + struct sockaddr_in addr; + int fd, index, on = 1; + + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_ANY); + addr.sin_port = htons(port); + + fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (fd < 0) { + perror("socket"); + goto no_socket; + } + index = interface_index(fd, name); + if (index < 0) + goto no_option; + + if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on))) { + perror("setsockopt SO_REUSEADDR"); + goto no_option; + } + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr))) { + perror("bind"); + goto no_option; + } + addr.sin_addr = *mc_addr; + if (mcast_join(fd, index, (struct sockaddr *) &addr, sizeof(addr))) { + perror("mcast_join"); + goto no_option; + } + return fd; +no_option: + close(fd); +no_socket: + return -1; +} + +static struct in_addr mc_addr; + +int udp_open(char *name, struct fdarray *fda, enum timestamp_type ts_type) +{ + int efd, gfd; + + if (!inet_aton(MULTICAST_IP_ADDR, &mc_addr)) + return -1; + + efd = open_socket(name, &mc_addr, EVENT_PORT); + if (efd < 0) + goto no_event; + + gfd = open_socket(name, &mc_addr, GENERAL_PORT); + if (gfd < 0) + goto no_general; + + if (timestamping_init(efd, name, ts_type)) + goto no_timestamping; + + fda->fd[FD_EVENT] = efd; + fda->fd[FD_GENERAL] = gfd; + fda->cnt = 2; + return 0; + +no_timestamping: + close(gfd); +no_general: + close(efd); +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, type; + struct cmsghdr *cm; + struct iovec iov = { buf, buflen }; + struct msghdr msg; + struct timespec *ts = NULL; + + memset(&msg, 0, sizeof(msg)); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); +again: + cnt = recvmsg(fd, &msg, flags); + if (cnt < 0) { + if (EINTR == errno) + goto again; + } + + 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) { + fprintf(stderr, "short SO_TIMESTAMPING\n"); + 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 udp_recv(int fd, void *buf, int buflen, struct hw_timestamp *hwts) +{ + return receive(fd, buf, buflen, hwts, 0); +} + +int udp_send(struct fdarray *fda, int event, + void *buf, int len, struct hw_timestamp *hwts) +{ + ssize_t cnt; + int fd = event ? fda->fd[FD_EVENT] : fda->fd[FD_GENERAL]; + struct sockaddr_in addr; + unsigned char junk[1600]; + + addr.sin_family = AF_INET; + addr.sin_addr = mc_addr; + addr.sin_port = htons(event ? EVENT_PORT : GENERAL_PORT); + + cnt = sendto(fd, buf, len, 0, (struct sockaddr *)&addr, sizeof(addr)); + if (cnt < 1) + return cnt; + /* + * Get the time stamp right away. + */ + return event ? receive(fd, junk, len, hwts, MSG_ERRQUEUE) : cnt; +} + +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) { + perror("socket"); + return -1; + } + + err = ioctl(fd, SIOCGIFHWADDR, &ifreq); + if (err < 0) { + perror("ioctl SIOCGIFHWADDR"); + close(fd); + return -1; + } + + memcpy(mac, ifreq.ifr_hwaddr.sa_data, len); + close(fd); + return 0; +} diff --git a/udp.h b/udp.h new file mode 100644 index 0000000..ad5e034 --- /dev/null +++ b/udp.h @@ -0,0 +1,41 @@ +/** + * @file udp.h + * @brief Implements transport over IPv4 UDP. + * @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_UPD_H +#define HAVE_UPD_H + +#include "fd.h" +#include "transport.h" + +int udp_close(struct fdarray *fda); +int udp_open(char *name, struct fdarray *fda, enum timestamp_type ts_type); +int udp_recv(int fd, void *buf, int buflen, struct hw_timestamp *hwts); +int udp_send(struct fdarray *fda, int event, + void *buf, int buflen, struct hw_timestamp *hwts); + +/** + * 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); + +#endif