From aea4cfcd507bcbc986bf52659621501992122540 Mon Sep 17 00:00:00 2001 From: Richard Cochran Date: Fri, 20 Apr 2012 16:30:01 +0200 Subject: [PATCH] Add transport over UDP IPv6. Signed-off-by: Richard Cochran --- makefile | 2 +- msg.h | 1 + transport.c | 3 +- udp6.c | 220 ++++++++++++++++++++++++++++++++++++++++++++++++++++ udp6.h | 32 ++++++++ 5 files changed, 256 insertions(+), 2 deletions(-) create mode 100644 udp6.c create mode 100644 udp6.h diff --git a/makefile b/makefile index e29754d..fdb3fd7 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 raw.o servo.o sk.o tmtab.o transport.o udp.o util.o + print.o raw.o servo.o sk.o tmtab.o transport.o udp.o udp6.o util.o OBJECTS = $(OBJ) phc2sys.o hwstamp_ctl.o SRC = $(OBJECTS:.o=.c) diff --git a/msg.h b/msg.h index 09bc9a4..c422e77 100644 --- a/msg.h +++ b/msg.h @@ -141,6 +141,7 @@ struct ptp_message { struct pdelay_resp_fup_msg pdelay_resp_fup; } PACKED; /**/ + int tail_room; int refcnt; TAILQ_ENTRY(ptp_message) list; struct { diff --git a/transport.c b/transport.c index 2d71e25..b0c80f4 100644 --- a/transport.c +++ b/transport.c @@ -21,6 +21,7 @@ #include "transport_private.h" #include "raw.h" #include "udp.h" +#include "udp6.h" int transport_close(struct transport *t, struct fdarray *fda) { @@ -57,7 +58,7 @@ struct transport *transport_create(enum transport_type type) case TRANS_UDP_IPV4: return udp_transport_create(); case TRANS_UDP_IPV6: - break; + return udp6_transport_create(); case TRANS_IEEE_802_3: return raw_transport_create(); case TRANS_DEVICENET: diff --git a/udp6.c b/udp6.c new file mode 100644 index 0000000..115cd83 --- /dev/null +++ b/udp6.c @@ -0,0 +1,220 @@ +/** + * @file udp6.c + * @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 +#include +#include + +#include "print.h" +#include "sk.h" +#include "transport_private.h" +#include "udp6.h" + +#define EVENT_PORT 319 +#define GENERAL_PORT 320 +#define PTP_PRIMARY_MCAST_IP6ADDR "FF0E:0:0:0:0:0:0:181" +#define PTP_PDELAY_MCAST_IP6ADDR "FF02:0:0:0:0:0:0:6B" + +static int mc_bind(int fd, int index) +{ + int err; + struct ipv6_mreq req; + memset(&req, 0, sizeof(req)); + req.ipv6mr_interface = index; + err = setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_IF, &req, sizeof(req)); + if (err) { + pr_err("setsockopt IPV6_MULTICAST_IF failed: %m"); + return -1; + } + return 0; +} + +static int mc_join(int fd, int index, const struct sockaddr *grp, socklen_t grplen) +{ + int err, off = 0; + struct ipv6_mreq req; + struct sockaddr_in6 *sa = (struct sockaddr_in6 *) grp; + + memset(&req, 0, sizeof(req)); + memcpy(&req.ipv6mr_multiaddr, &sa->sin6_addr, sizeof(struct in6_addr)); + req.ipv6mr_interface = index; + err = setsockopt(fd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &req, sizeof(req)); + if (err) { + pr_err("setsockopt IPV6_ADD_MEMBERSHIP failed: %m"); + return -1; + } + err = setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &off, sizeof(off)); + if (err) { + pr_err("setsockopt IPV6_MULTICAST_LOOP failed: %m"); + return -1; + } + return 0; +} + +static int udp6_close(struct transport *t, struct fdarray *fda) +{ + close(fda->fd[0]); + close(fda->fd[1]); + return 0; +} + +static int open_socket_ipv6(char *name, struct in6_addr mc_addr[2], short port) +{ + struct sockaddr_in6 addr; + int fd, index, on = 1; + + addr.sin6_family = AF_INET6; + addr.sin6_addr = in6addr_any; + addr.sin6_port = htons(port); + + fd = socket(PF_INET6, SOCK_DGRAM, IPPROTO_UDP); + if (fd < 0) { + pr_err("socket failed: %m"); + goto no_socket; + } + index = sk_interface_index(fd, name); + if (index < 0) + goto no_option; + + if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on))) { + pr_err("setsockopt SO_REUSEADDR failed: %m"); + goto no_option; + } + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr))) { + pr_err("bind failed: %m"); + goto no_option; + } + if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, name, strlen(name))) { + pr_err("setsockopt SO_BINDTODEVICE failed: %m"); + goto no_option; + } + addr.sin6_addr = mc_addr[0]; + if (mc_join(fd, index, (struct sockaddr *) &addr, sizeof(addr))) { + pr_err("mcast_join failed"); + goto no_option; + } + addr.sin6_addr = mc_addr[1]; + if (mc_join(fd, index, (struct sockaddr *) &addr, sizeof(addr))) { + pr_err("mcast_join failed"); + goto no_option; + } + if (mc_bind(fd, index)) { + goto no_option; + } + return fd; +no_option: + close(fd); +no_socket: + return -1; +} + +enum { MC_PRIMARY, MC_PDELAY }; + +static struct in6_addr mc6_addr[2]; + +static int udp6_open(struct transport *t, char *name, struct fdarray *fda, + enum timestamp_type ts_type) +{ + int efd, gfd; + + if (1 != inet_pton(AF_INET6, PTP_PRIMARY_MCAST_IP6ADDR, &mc6_addr[MC_PRIMARY])) + return -1; + + if (1 != inet_pton(AF_INET6, PTP_PDELAY_MCAST_IP6ADDR, &mc6_addr[MC_PDELAY])) + return -1; + + efd = open_socket_ipv6(name, mc6_addr, EVENT_PORT); + if (efd < 0) + goto no_event; + + gfd = open_socket_ipv6(name, mc6_addr, GENERAL_PORT); + if (gfd < 0) + goto no_general; + + if (sk_timestamping_init(efd, name, ts_type)) + goto no_timestamping; + + fda->fd[FD_EVENT] = efd; + fda->fd[FD_GENERAL] = gfd; + return 0; + +no_timestamping: + close(gfd); +no_general: + close(efd); +no_event: + return -1; +} + +static int udp6_recv(struct transport *t, int fd, void *buf, int buflen, + struct hw_timestamp *hwts) +{ + return sk_receive(fd, buf, buflen, hwts, 0); +} + +static int udp6_send(struct transport *t, struct fdarray *fda, int event, int peer, + void *buf, int len, struct hw_timestamp *hwts) +{ + ssize_t cnt; + int fd = event ? fda->fd[FD_EVENT] : fda->fd[FD_GENERAL]; + struct sockaddr_in6 addr; + unsigned char junk[1600]; + + addr.sin6_family = AF_INET6; + addr.sin6_addr = peer ? mc6_addr[MC_PDELAY] : mc6_addr[MC_PRIMARY]; + addr.sin6_port = htons(event ? EVENT_PORT : GENERAL_PORT); + + len += 2; /* Extend the payload by two, for UDP checksum corrections. */ + + cnt = sendto(fd, buf, len, 0, (struct sockaddr *)&addr, sizeof(addr)); + if (cnt < 1) { + pr_err("sendto failed: %m"); + return cnt; + } + /* + * Get the time stamp right away. + */ + return event ? sk_receive(fd, junk, len, hwts, MSG_ERRQUEUE) : cnt; +} + +static void udp6_release(struct transport *t) +{ + /* No need for any per-instance deallocation. */ +} + +static struct transport the_udp6_transport = { + .close = udp6_close, + .open = udp6_open, + .recv = udp6_recv, + .send = udp6_send, + .release = udp6_release, +}; + +struct transport *udp6_transport_create(void) +{ + /* No need for any per-instance allocation. */ + return &the_udp6_transport; +} diff --git a/udp6.h b/udp6.h new file mode 100644 index 0000000..8613da5 --- /dev/null +++ b/udp6.h @@ -0,0 +1,32 @@ +/** + * @file udp6.h + * @brief Implements transport over IPv6 UDP. + * @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_UPD6_H +#define HAVE_UPD6_H + +#include "fd.h" +#include "transport.h" + +/** + * Allocate an instance of a UDP/IPv6 transport. + * @return Pointer to a new transport instance on success, NULL otherwise. + */ +struct transport *udp6_transport_create(void); + +#endif