From: Amit Nishry <am...@mellanox.com>
This allows to configure different VLANs on the same physical port without an
IP address. RAW sockets and the BPF filter are the basis of the transport.
New port section configration:
[eth1@10]
network_transport RAWUDPv4
vlan 10
vlan_intf vlan10
Port section has a '@' to indicate the VLAN ID of the port.
vlan_intf option should specify an L3 interface with configured IP address to
be used as source address of the packets sent from ports in the VLAN.
Signed-off-by: Amit Nishry <am...@mellanox.com>
Signed-off-by: Andriy Kohut <andr...@mellanox.com>
---
clock.c | 7 +-
config.c | 18 +-
config.h | 2 +
makefile | 14 +-
msg.c | 2 +-
phc2sys.c | 25 ++-
ptp4l.8 | 11 +-
raw_udp.c | 540 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
raw_udp.h | 32 ++++
sk.c | 1 +
transport.c | 4 +
transport.h | 1 +
util.c | 1 +
13 files changed, 640 insertions(+), 18 deletions(-)
create mode 100644 raw_udp.c
create mode 100644 raw_udp.h
diff --git a/clock.c b/clock.c
index 2400b2f..745ba9a 100644
--- a/clock.c
+++ b/clock.c
@@ -43,6 +43,7 @@
#include "tsproc.h"
#include "uds.h"
#include "util.h"
+#include "hash.h"
#define N_CLOCK_PFD (N_POLLFD + 1) /* one extra per port, for the fault timer
*/
#define POW2_41 ((double)(1ULL << 41))
@@ -842,7 +843,7 @@ int clock_required_modes(struct clock *c)
static void ensure_ts_label(struct interface *iface)
{
if (iface->ts_label[0] == '\0')
- strncpy(iface->ts_label, iface->name, MAX_IFNAME_SIZE);
+ strncpy(iface->ts_label, iface->netdev, MAX_IFNAME_SIZE);
}
struct clock *clock_create(enum clock_type type, struct config *config,
@@ -958,7 +959,7 @@ struct clock *clock_create(enum clock_type type, struct
config *config,
if (iface->ts_info.valid &&
((iface->ts_info.so_timestamping & required_modes) !=
required_modes)) {
pr_err("interface '%s' does not support "
- "requested timestamping mode", iface->name);
+ "requested timestamping mode", iface->netdev);
return NULL;
}
}
@@ -986,7 +987,7 @@ struct clock *clock_create(enum clock_type type, struct
config *config,
pr_info("selected /dev/ptp%d as PTP clock", phc_index);
}
- if (generate_clock_identity(&c->dds.clockIdentity, iface->name)) {
+ if (generate_clock_identity(&c->dds.clockIdentity, iface->netdev)) {
pr_err("failed to generate a clock identity");
return NULL;
}
diff --git a/config.c b/config.c
index 7914ba4..7f3ca9b 100644
--- a/config.c
+++ b/config.c
@@ -165,9 +165,10 @@ static struct config_enum delay_mech_enu[] = {
};
static struct config_enum nw_trans_enu[] = {
- { "L2", TRANS_IEEE_802_3 },
- { "UDPv4", TRANS_UDP_IPV4 },
- { "UDPv6", TRANS_UDP_IPV6 },
+ { "L2", TRANS_IEEE_802_3 },
+ { "UDPv4", TRANS_UDP_IPV4 },
+ { "UDPv6", TRANS_UDP_IPV6 },
+ { "RAWUDPv4", TRANS_RAW_UDP_IPV4 },
{ NULL, 0 },
};
@@ -273,6 +274,8 @@ struct config_item config_tab[] = {
GLOB_ITEM_STR("userDescription", ""),
GLOB_ITEM_INT("utc_offset", CURRENT_UTC_OFFSET, 0, INT_MAX),
GLOB_ITEM_INT("verbose", 0, 0, 1),
+ PORT_ITEM_INT("vlan", DEFAULT_VLAN, DEFAULT_VLAN, 4095),
+ PORT_ITEM_STR("vlan_intf", "lo"),
};
static struct unicast_master_table *current_uc_mtab;
@@ -787,6 +790,7 @@ parse_error:
struct interface *config_create_interface(char *name, struct config *cfg)
{
struct interface *iface;
+ char *vid = NULL;
/* only create each interface once (by name) */
STAILQ_FOREACH(iface, &cfg->interfaces, list) {
@@ -801,6 +805,14 @@ struct interface *config_create_interface(char *name,
struct config *cfg)
}
strncpy(iface->name, name, MAX_IFNAME_SIZE);
+ strncpy(iface->netdev, name, MAX_IFNAME_SIZE);
+
+ vid = strchr(iface->netdev, '@');
+ if (vid != NULL) {
+ *vid = '\0';
+ }
+
+ sk_get_ts_info(iface->netdev, &iface->ts_info);
STAILQ_INSERT_TAIL(&cfg->interfaces, iface, list);
cfg->n_interfaces++;
diff --git a/config.h b/config.h
index f237fb2..0a2badb 100644
--- a/config.h
+++ b/config.h
@@ -32,6 +32,7 @@
#include "sk.h"
#define MAX_IFNAME_SIZE 108 /* = UNIX_PATH_MAX */
+#define DEFAULT_VLAN 1
#if (IF_NAMESIZE > MAX_IFNAME_SIZE)
#error if_namesize larger than expected.
@@ -42,6 +43,7 @@ struct interface {
STAILQ_ENTRY(interface) list;
char name[MAX_IFNAME_SIZE + 1];
char ts_label[MAX_IFNAME_SIZE + 1];
+ char netdev[MAX_IFNAME_SIZE + 1];
struct sk_ts_info ts_info;
};
diff --git a/makefile b/makefile
index 693e75d..d324a13 100644
--- a/makefile
+++ b/makefile
@@ -27,7 +27,8 @@ OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o
e2e_tc.o fault.o \
filter.o fsm.o hash.o linreg.o mave.o mmedian.o msg.o ntpshm.o nullf.o phc.o \
pi.o port.o port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.o raw.o rtnl.o \
servo.o sk.o stats.o tc.o telecom.o tlv.o transport.o tsproc.o udp.o udp6.o \
- uds.o unicast_client.o unicast_fsm.o unicast_service.o util.o version.o
+ uds.o unicast_client.o unicast_fsm.o unicast_service.o util.o version.o \
+ raw_udp.o
OBJECTS = $(OBJ) hwstamp_ctl.o nsm.o phc2sys.o phc_ctl.o pmc.o
pmc_common.o \
sysoff.o timemaster.o
@@ -48,14 +49,15 @@ all: $(PRG)
ptp4l: $(OBJ)
nsm: config.o filter.o hash.o mave.o mmedian.o msg.o nsm.o print.o raw.o \
- rtnl.o sk.o transport.o tlv.o tsproc.o udp.o udp6.o uds.o util.o version.o
+ raw_udp.o rtnl.o sk.o transport.o tlv.o tsproc.o udp.o udp6.o uds.o util.o \
+ version.o
-pmc: config.o hash.o 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
+pmc: config.o hash.o msg.o pmc.o pmc_common.o print.o raw.o raw_udp.o \
+ sk.o tlv.o transport.o udp.o udp6.o uds.o util.o version.o
phc2sys: clockadj.o clockcheck.o config.o hash.o linreg.o msg.o ntpshm.o \
- nullf.o phc.o phc2sys.o pi.o pmc_common.o print.o raw.o servo.o sk.o stats.o \
- sysoff.o tlv.o transport.o udp.o udp6.o uds.o util.o version.o
+ nullf.o phc.o phc2sys.o pi.o pmc_common.o print.o raw.o raw_udp.o servo.o \
+ sk.o stats.o sysoff.o tlv.o transport.o udp.o udp6.o uds.o util.o version.o
hwstamp_ctl: hwstamp_ctl.o version.o
diff --git a/msg.c b/msg.c
index 6c20c97..448a9b8 100644
--- a/msg.c
+++ b/msg.c
@@ -37,7 +37,7 @@ int assume_two_step = 0;
/*
* Head room fits a VLAN Ethernet header, and 'msg' is 64 bit aligned.
*/
-#define MSG_HEADROOM 24
+#define MSG_HEADROOM 24 + 28 + 16
struct message_storage {
unsigned char reserved[MSG_HEADROOM];
diff --git a/phc2sys.c b/phc2sys.c
index 66e34d6..3e5fa38 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -135,36 +135,50 @@ static int run_pmc_port_properties(struct node *node, int
timeout,
unsigned int port,
int *state, int *tstamping, char *iface);
+static void check_netdev_name(char *name) {
+ /* strip VLAN if it's VLAN member PTP port */
+ char *vid = strchr(name, '@');
+ if (vid != NULL) {
+ *vid = '\0';
+ }
+}
+
+
static clockid_t clock_open(char *device, int *phc_index)
{
struct sk_ts_info ts_info;
char phc_device[16];
+ char netdev[MAX_IFNAME_SIZE + 1] = {};
int clkid;
/* check if device is CLOCK_REALTIME */
if (!strcasecmp(device, "CLOCK_REALTIME"))
return CLOCK_REALTIME;
+ strncpy(netdev, device, MAX_IFNAME_SIZE);
+
+ check_netdev_name(netdev);
+
/* check if device is valid phc device */
clkid = phc_open(device);
if (clkid != CLOCK_INVALID)
return clkid;
/* check if device is a valid ethernet device */
- if (sk_get_ts_info(device, &ts_info) || !ts_info.valid) {
- fprintf(stderr, "unknown clock %s: %m\n", device);
+ if (sk_get_ts_info(netdev, &ts_info) || !ts_info.valid) {
+ fprintf(stderr, "unknown clock %s: %m\n", netdev);
return CLOCK_INVALID;
}
if (ts_info.phc_index < 0) {
- fprintf(stderr, "interface %s does not have a PHC\n", device);
+ fprintf(stderr, "interface %s does not have a PHC\n", netdev);
return CLOCK_INVALID;
}
sprintf(phc_device, "/dev/ptp%d", ts_info.phc_index);
clkid = phc_open(phc_device);
if (clkid == CLOCK_INVALID)
- fprintf(stderr, "cannot open %s: %m\n", device);
+ fprintf(stderr, "cannot open %s: %m\n", netdev);
*phc_index = ts_info.phc_index;
return clkid;
}
@@ -369,6 +383,9 @@ static void clock_reinit(struct node *node, struct clock
*clock, int new_state)
free(clock->device);
clock->device = strdup(iface);
}
+
+ check_netdev_name(clock->device);
+
/* Check if phc index changed */
if (!sk_get_ts_info(clock->device, &ts_info) &&
clock->phc_index != ts_info.phc_index) {
diff --git a/ptp4l.8 b/ptp4l.8
index 10c5c2f..74d2dc2 100644
--- a/ptp4l.8
+++ b/ptp4l.8
@@ -275,7 +275,7 @@ The MAC address to which peer delay messages should be sent.
Relevant only with L2 transport. The default is 01:80:C2:00:00:0E.
.TP
.B network_transport
-Select the network transport. Possible values are UDPv4, UDPv6 and L2.
+Select the network transport. Possible values are UDPv4, RAWUDPv4, UDPv6 and
L2.
The default is UDPv4.
.TP
.B neighborPropDelayThresh
@@ -353,6 +353,15 @@ limit for IPv6 multicast messages. This option is only
relevant with the IPv4
and IPv6 UDP transports. The default is 1 to restrict the messages sent by
.B ptp4l
to the same subnet.
+.TP
+.B vlan
+Specifies the VLAN of the port to operate in. Applicable with RAWUDPv4
+transport only. The default is 1 (untagged packets).
+.TP
+.B vlan_intf
+Specifies the VLAN interface name of the port that is part of that VLAN.
+IP address of the VLAN interface will be used as source address for the
packets.
+The default is "lo" (loopback interface).
.SH PROGRAM AND CLOCK OPTIONS
diff --git a/raw_udp.c b/raw_udp.c
new file mode 100644
index 0000000..6a6c550
--- /dev/null
+++ b/raw_udp.c
@@ -0,0 +1,540 @@
+/**
+ * @file raw_udp.c
+ * @note Copyright (C) 2018 Richard Cochran <richardcoch...@gmail.com>
+ *
+ * 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 <arpa/inet.h>
+#include <errno.h>
+#include <linux/filter.h>
+#include <linux/if_ether.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include "address.h"
+#include "config.h"
+#include "contain.h"
+#include "ether.h"
+#include "print.h"
+#include "raw_udp.h"
+#include "sk.h"
+#include "transport_private.h"
+#include "util.h"
+
+struct udp_hdr {
+ uint16_t udp_src;
+ uint16_t udp_dst;
+ uint16_t udp_len;
+ uint16_t udp_check;
+} __attribute__((packed));
+
+struct ip_hdr {
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+ uint8_t ip_hl:4,
+ ip_v:4;
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+ uint8_t ip_v:4,
+ ip_hl:4;
+#endif
+ uint8_t ip_tos;
+ uint16_t ip_len;
+ uint16_t ip_id;
+ uint16_t ip_off;
+ uint8_t ip_ttl;
+ uint8_t ip_p;
+ uint16_t ip_check;
+ struct in_addr ip_src;
+ struct in_addr ip_dst;
+} __attribute__((packed));
+
+struct pseudo_hdr {
+ uint32_t ip_src;
+ uint32_t ip_dst;
+ uint8_t reserved;
+ uint8_t protocol;
+ uint16_t length;
+} __attribute__((packed));
+
+struct raw_udp {
+ struct transport t;
+ struct address src_addr;
+ struct address ip_src;
+ struct address ptp_addr;
+ struct address p2p_addr;
+ uint32_t vlan;
+ uint8_t dscp_general;
+ uint8_t dscp_event;
+};
+
+#define EVENT_PORT 319
+#define GENERAL_PORT 320
+#define PTP_PRIMARY_MCAST_IPADDR "224.0.1.129"
+#define PTP_PDELAY_MCAST_IPADDR "224.0.0.107"
+
+#define OP_AND (BPF_ALU | BPF_AND | BPF_K)
+#define OP_ADDX (BPF_ALU | BPF_ADD | BPF_X)
+#define OP_JEQ (BPF_JMP | BPF_JEQ | BPF_K)
+#define OP_JUN (BPF_JMP | BPF_JA)
+#define OP_LDB (BPF_LD | BPF_B | BPF_ABS)
+#define OP_LDBIN (BPF_LD | BPF_B | BPF_IND)
+#define OP_LDH (BPF_LD | BPF_H | BPF_ABS)
+#define OP_LDHIN (BPF_LD | BPF_H | BPF_IND)
+#define OP_LDX (BPF_LDX)
+#define OP_LDXB (BPF_LDX | BPF_B | BPF_MSH)
+#define OP_JSET (BPF_JMP | BPF_JSET)
+#define OP_JMP (BPF_JMP)
+#define OP_RETK (BPF_RET | BPF_K)
+#define OP_TXA (BPF_MISC | BPF_TXA)
+#define OP_TAX (BPF_MISC | BPF_TAX)
+
+#define VLAN_AVAILABLE (SKF_AD_OFF + SKF_AD_VLAN_TAG_PRESENT)
+#define VLAN_IDENTIFIER (SKF_AD_OFF + SKF_AD_VLAN_TAG)
+
+#define OFF_IP_PROTO 23
+#define OFF_IP_FRAG 20
+#define OFF_IP_HLEN 14
+#define OFF_DPORT_REL 16
+#define IP_FRAG_PART 0x1FFF
+
+#define N_TAGGED_FILTER 25
+
+#define VLAN_TEST 3
+#define TAGGED_PORT_TEST 22
+
+enum { MC_PRIMARY, MC_PDELAY };
+static struct in_addr mcast_addr[2];
+
+/* On some systems packets may come to the kernel without 802.1Q tag.
+ * For that case BPF code below checks for respective EtherType and
+ * puts 4 to the X register if tag is present or 0 otherwise.
+ * Further operations use indirect addressing with X register. */
+static struct sock_filter tagged_filter[N_TAGGED_FILTER] = {
+ { OP_LDB, 0, 0, VLAN_AVAILABLE },
+ { OP_JEQ, 0, 22, 1 }, /*f goto reject*/
+ { OP_LDB, 0, 0, VLAN_IDENTIFIER },
+ { OP_JEQ, 0, 20, 1 }, /*test VID, f goto reject*/
+ { OP_LDH, 0, 0, OFF_ETYPE },
+ { OP_LDX, 0, 0, 0 },
+ { OP_JEQ, 0, 1, ETH_P_8021Q },
+ { OP_LDX, 0, 0, 4 }, /*tag is present*/
+ { OP_LDHIN, 0, 0, OFF_ETYPE },
+ { OP_JEQ, 0, 14, ETH_P_IP }, /*f goto reject*/
+ { OP_LDBIN, 0, 0, OFF_IP_PROTO },
+ { OP_JEQ, 0, 12, IPPROTO_UDP }, /*f goto reject*/
+ { OP_LDHIN, 0, 0, OFF_IP_FRAG },
+ { OP_JSET, 10, 0, IP_FRAG_PART }, /*t goto reject*/
+ { OP_TXA, 0, 0, 0 },
+ { OP_JEQ, 2, 0, 0 }, /*t w/o tag, goto len offset*/
+ { OP_LDXB, 0, 0, OFF_IP_HLEN + 4 },
+ { OP_JMP, 0, 0, 1 },
+ { OP_LDXB, 0, 0, OFF_IP_HLEN },
+ { OP_ADDX, 0, 0, 0 },
+ { OP_TAX, 0, 0, 0 },
+ { OP_LDHIN, 0, 0, OFF_DPORT_REL },
+ { OP_JEQ, 0, 1, GENERAL_PORT }, /*test PTP port*/
+ { OP_RETK, 0, 0, 1500 }, /*accept*/
+ { OP_RETK, 0, 0, 0000000000 }, /*reject*/
+};
+
+static int raw_udp_configure(int fd, int ev, int index, unsigned char *addr1,
+ unsigned char *addr2, int op, unsigned int vlan)
+{
+ int err1, err2, option;
+ struct packet_mreq mreq;
+ struct sock_fprog prg;
+
+ tagged_filter[VLAN_TEST].k = vlan;
+ tagged_filter[TAGGED_PORT_TEST].k = ev ? EVENT_PORT :
+ GENERAL_PORT;
+ prg.len = N_TAGGED_FILTER;
+ prg.filter = tagged_filter;
+
+ if (setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, &prg, sizeof(prg))) {
+ pr_err("setsockopt SO_ATTACH_FILTER failed: %m");
+ return -1;
+ }
+
+ option = op ? PACKET_ADD_MEMBERSHIP : PACKET_DROP_MEMBERSHIP;
+
+ memset(&mreq, 0, sizeof(mreq));
+ mreq.mr_ifindex = index;
+ mreq.mr_type = PACKET_MR_MULTICAST;
+ mreq.mr_alen = MAC_LEN;
+ memcpy(mreq.mr_address, addr1, MAC_LEN);
+
+ err1 = setsockopt(fd, SOL_PACKET, option, &mreq, sizeof(mreq));
+ if (err1)
+ pr_warning("setsockopt PACKET_MR_MULTICAST failed: %m");
+
+ memcpy(mreq.mr_address, addr2, MAC_LEN);
+
+ err2 = setsockopt(fd, SOL_PACKET, option, &mreq, sizeof(mreq));
+ if (err2)
+ pr_warning("setsockopt PACKET_MR_MULTICAST failed: %m");
+
+ if (!err1 && !err2)
+ return 0;
+
+ mreq.mr_ifindex = index;
+ mreq.mr_type = PACKET_MR_ALLMULTI;
+ mreq.mr_alen = 0;
+ if (!setsockopt(fd, SOL_PACKET, option, &mreq, sizeof(mreq))) {
+ pr_warning("setsockopt PACKET_MR_ALLMULTI failed: %m");
+ return 0;
+ }
+
+ mreq.mr_ifindex = index;
+ mreq.mr_type = PACKET_MR_PROMISC;
+ mreq.mr_alen = 0;
+ if (!setsockopt(fd, SOL_PACKET, option, &mreq, sizeof(mreq))) {
+ pr_warning("setsockopt PACKET_MR_PROMISC failed: %m");
+ return 0;
+ }
+
+ pr_err("all socket options failed");
+ return -1;
+}
+
+static int raw_udp_close(struct transport *t, struct fdarray *fda)
+{
+ close(fda->fd[0]);
+ close(fda->fd[1]);
+ return 0;
+}
+
+static int open_socket(const char *name, int ev, unsigned char *ptp_dst_mac,
+ unsigned char *p2p_dst_mac, unsigned int vlan)
+{
+ struct sockaddr_ll addr;
+ int fd, idx;
+
+ fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
+ if (fd < 0) {
+ pr_err("socket failed: %m");
+ goto no_socket;
+ }
+ idx = sk_interface_index(fd, name);
+ if (idx < 0)
+ goto no_option;
+
+ memset(&addr, 0, sizeof(addr));
+ addr.sll_ifindex = idx;
+ addr.sll_family = AF_PACKET;
+ addr.sll_protocol = htons(ETH_P_ALL);
+ 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;
+ }
+ if (raw_udp_configure(fd, ev, idx, ptp_dst_mac, p2p_dst_mac, 1, vlan))
+ goto no_option;
+
+ return fd;
+no_option:
+ close(fd);
+no_socket:
+ return -1;
+}
+
+static void mac_to_addr(struct address *addr, void *mac)
+{
+ addr->sll.sll_family = AF_PACKET;
+ addr->sll.sll_halen = MAC_LEN;
+ memcpy(addr->sll.sll_addr, mac, MAC_LEN);
+ addr->len = sizeof(addr->sll);
+}
+
+static void addr_to_mac(void *mac, struct address *addr)
+{
+ memcpy(mac, &addr->sll.sll_addr, MAC_LEN);
+}
+
+static void mc_ip_to_addr(struct in_addr ip_addr, unsigned char mac[MAC_LEN])
+{
+ uint32_t addr = htonl(ip_addr.s_addr);
+
+ mac[0] = 0x1;
+ mac[1] = 0x0;
+ mac[2] = 0x5E;
+ mac[3] = (addr & 0x007F0000) >> 16;
+ mac[4] = (addr & 0x0000FF00) >> 8;
+ mac[5] = addr & 0x000000FF;
+}
+
+static int raw_udp_open(struct transport *t, struct interface *iface,
+ struct fdarray *fda, enum timestamp_type type)
+{
+ struct raw_udp *raw = container_of(t, struct raw_udp, t);
+ unsigned char ptp_dst_mac[MAC_LEN];
+ unsigned char p2p_dst_mac[MAC_LEN];
+ int efd, gfd;
+ char *str;
+ unsigned int vlan = 0;
+ char *vlan_intf = config_get_string(t->cfg, iface->name, "vlan_intf");
+
+ raw->vlan = vlan = config_get_int(t->cfg, iface->name, "vlan");
+ raw->dscp_event = config_get_int(t->cfg, iface->name, "dscp_event");
+ raw->dscp_general = config_get_int(t->cfg, iface->name, "dscp_general");
+
+ str = config_get_string(t->cfg, iface->name, "p2p_dst_mac");
+ if (str2mac(str, p2p_dst_mac)) {
+ pr_err("invalid p2p_dst_mac %s", str);
+ return -1;
+ }
+ if (!inet_aton(PTP_PRIMARY_MCAST_IPADDR, &mcast_addr[MC_PRIMARY]))
+ return -1;
+
+ if (!inet_aton(PTP_PDELAY_MCAST_IPADDR, &mcast_addr[MC_PDELAY]))
+ return -1;
+
+ if (sk_interface_addr(vlan_intf, AF_INET, &raw->ip_src)) {
+ pr_err("failed to get IP address of %s", vlan_intf);
+ goto no_addr;
+ }
+
+ mc_ip_to_addr(mcast_addr[MC_PRIMARY], ptp_dst_mac);
+ mc_ip_to_addr(mcast_addr[MC_PDELAY], p2p_dst_mac);
+
+ mac_to_addr(&raw->ptp_addr, ptp_dst_mac);
+ mac_to_addr(&raw->p2p_addr, p2p_dst_mac);
+
+ if (sk_interface_macaddr(vlan_intf, &raw->src_addr))
+ goto no_addr;
+
+ efd = open_socket(iface->netdev, 1, ptp_dst_mac, p2p_dst_mac, vlan);
+ if (efd < 0)
+ goto no_event;
+
+ gfd = open_socket(iface->netdev, 0, ptp_dst_mac, p2p_dst_mac, vlan);
+ if (gfd < 0)
+ goto no_general;
+
+ if (sk_timestamping_init(efd, iface->netdev, type, TRANS_RAW_UDP_IPV4))
+ goto no_timestamping;
+
+ if (sk_general_init(gfd))
+ 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:
+no_addr:
+ return -1;
+}
+
+static int raw_udp_recv(struct transport *t, int fd, void *buf, int buflen,
+ struct address *addr, struct hw_timestamp *hwts)
+{
+ int cnt, hlen;
+ unsigned char *ptr = buf;
+
+ hlen = sizeof(struct eth_hdr) + sizeof(struct ip_hdr) +
+ sizeof(struct udp_hdr);
+
+ ptr -= hlen;
+ buflen += hlen;
+
+ cnt = sk_receive(fd, ptr, buflen, addr, hwts, 0);
+
+ if (cnt >= 0)
+ cnt -= hlen;
+ return cnt;
+}
+
+/*www.microhowto.info/howto/calculate_an_internet_protocol_checksum_in_c.html*/
+static uint16_t checksum(void *buf, int length)
+{
+ char *data = (char *)buf;
+
+ uint64_t acc = 0xffff;
+
+ unsigned int offset = ((uintptr_t)data)&3;
+ if (offset) {
+ size_t count = 4-offset;
+ if (count > length)
+ count = length;
+ uint32_t word = 0;
+ memcpy(offset + (char *) &word, data, count);
+ acc += ntohl(word);
+ data += count;
+ length -= count;
+ }
+
+ char *data_end = data + (length&~3);
+ while (data != data_end) {
+ uint32_t word;
+ memcpy(&word, data, 4);
+ acc += ntohl(word);
+ data += 4;
+ }
+ length &= 3;
+
+ if (length) {
+ uint32_t word = 0;
+ memcpy(&word, data, length);
+ acc += ntohl(word);
+ }
+
+ acc = (acc & 0xffffffff) + (acc >> 32);
+ while (acc >> 16)
+ acc = (acc & 0xffff) + (acc >> 16);
+
+ if (offset & 1)
+ acc = ((acc & 0xff00) >> 8) | ((acc & 0x00ff) << 8);
+
+ return htons(~acc);
+}
+
+static uint16_t udp_csum(struct ip_hdr *ip_hdr, void *buf, int udp_length)
+{
+ unsigned char pkt[1600] = {0};
+ struct pseudo_hdr ps_hdr = {0};
+
+ ps_hdr.ip_src = ip_hdr->ip_src.s_addr;
+ ps_hdr.ip_dst = ip_hdr->ip_dst.s_addr;
+ ps_hdr.reserved = 0;
+ ps_hdr.protocol = IPPROTO_UDP;
+ ps_hdr.length = htons(udp_length);
+
+ memcpy(pkt, &ps_hdr, sizeof(ps_hdr));
+ memcpy(pkt + sizeof(ps_hdr), buf, udp_length);
+
+ return checksum(pkt, sizeof(ps_hdr) + udp_length);
+}
+
+static void raw_udp_packet_construct(struct raw_udp *raw, struct address *addr,
+ enum transport_event event, int peer,
+ int *len, unsigned char **ptr) {
+ struct vlan_hdr *vlan_hdr;
+ struct ip_hdr *ip_hdr;
+ struct udp_hdr *udp_hdr;
+
+ /* UDP Header */
+ *ptr -= sizeof(*udp_hdr);
+ *len += sizeof(*udp_hdr);
+
+ udp_hdr = (struct udp_hdr *) *ptr;
+ udp_hdr->udp_src = event ? htons(EVENT_PORT) : htons(GENERAL_PORT);
+ udp_hdr->udp_dst = event ? htons(EVENT_PORT) : htons(GENERAL_PORT);
+ udp_hdr->udp_len = htons(*len);
+
+ /* IP Header */
+ *ptr -= sizeof(*ip_hdr);
+ ip_hdr = (struct ip_hdr *) *ptr;
+
+ ip_hdr->ip_p = IPPROTO_UDP;
+ ip_hdr->ip_src = raw->ip_src.sin.sin_addr;
+ ip_hdr->ip_dst.s_addr = mcast_addr[MC_PRIMARY].s_addr;
+ ip_hdr->ip_ttl = 128;
+
+ udp_hdr->udp_check = 0;
+ udp_hdr->udp_check = udp_csum(ip_hdr, *ptr + sizeof(*ip_hdr), *len);
+
+ *len += sizeof(*ip_hdr);
+ ip_hdr->ip_v = 4;
+ ip_hdr->ip_hl = 5;
+ ip_hdr->ip_tos = event ? raw->dscp_event : raw->dscp_general;
+ ip_hdr->ip_tos <<= 2;
+ ip_hdr->ip_len = htons(*len);
+ ip_hdr->ip_id = htons(1);
+ ip_hdr->ip_off = htons(0x4000);
+ ip_hdr->ip_check = 0;
+ ip_hdr->ip_check = checksum(*ptr, ip_hdr->ip_hl * 4);
+
+ if (!addr)
+ addr = peer ? &raw->p2p_addr : &raw->ptp_addr;
+
+ /* Ethernet VLAN Header */
+ *ptr -= sizeof(*vlan_hdr);
+ *len += sizeof(*vlan_hdr);
+ vlan_hdr = (struct vlan_hdr *) *ptr;
+
+ addr_to_mac(&vlan_hdr->dst, addr);
+ addr_to_mac(&vlan_hdr->src, &raw->src_addr);
+ vlan_hdr->tpid = htons(ETH_P_8021Q);
+ vlan_hdr->tci = htons(raw->vlan);
+ vlan_hdr->type = htons(ETH_P_IP);
+}
+
+static int raw_udp_send(struct transport *t, struct fdarray *fda,
+ enum transport_event event, int peer, void *buf,
+ int len, struct address *addr,
+ struct hw_timestamp *hwts)
+{
+ struct raw_udp *raw = container_of(t, struct raw_udp, t);
+ ssize_t cnt;
+ int fd = event ? fda->fd[FD_EVENT] : fda->fd[FD_GENERAL];
+ unsigned char pkt[1500], *ptr = buf;
+
+ raw_udp_packet_construct(raw, addr, event, peer, &len, &ptr);
+
+ cnt = send(fd, ptr, len, 0);
+ if (cnt < 1) {
+ pr_err("send failed: %d %m", errno);
+ return cnt;
+ }
+ /*
+ * Get the time stamp right away.
+ */
+ if (event == TRANS_EVENT)
+ cnt = sk_receive(fd, pkt, len, NULL, hwts, MSG_ERRQUEUE);
+
+ return cnt;
+}
+
+static void raw_udp_release(struct transport *t)
+{
+ struct raw_udp *raw = container_of(t, struct raw_udp, t);
+ free(raw);
+}
+
+static int raw_udp_physical_addr(struct transport *t, uint8_t *addr)
+{
+ struct raw_udp *raw = container_of(t, struct raw_udp, t);
+ addr_to_mac(addr, &raw->src_addr);
+ return MAC_LEN;
+}
+
+static int raw_udp_protocol_addr(struct transport *t, uint8_t *addr)
+{
+ struct raw_udp *raw = container_of(t, struct raw_udp, t);
+ addr_to_mac(addr, &raw->src_addr);
+ return MAC_LEN;
+}
+
+struct transport *raw_udp_transport_create(void)
+{
+ struct raw_udp *raw;
+ raw = calloc(1, sizeof(*raw));
+ if (!raw)
+ return NULL;
+ raw->t.close = raw_udp_close;
+ raw->t.open = raw_udp_open;
+ raw->t.recv = raw_udp_recv;
+ raw->t.send = raw_udp_send;
+ raw->t.release = raw_udp_release;
+ raw->t.physical_addr = raw_udp_physical_addr;
+ raw->t.protocol_addr = raw_udp_protocol_addr;
+ return &raw->t;
+}
diff --git a/raw_udp.h b/raw_udp.h
new file mode 100644
index 0000000..fafee48
--- /dev/null
+++ b/raw_udp.h
@@ -0,0 +1,32 @@
+/**
+ * @file raw.h
+ * @brief Implements transport over IEEE 802.3 aka raw Ethernet.
+ * @note Copyright (C) 2018 Richard Cochran <richardcoch...@gmail.com>
+ *
+ * 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_RAW_UDP_H
+#define HAVE_RAW_UDP_H
+
+#include "fd.h"
+#include "transport.h"
+
+/**
+ * Allocate an instance of a raw UDP transport.
+ * @return Pointer to a new transport instance on success, NULL otherwise.
+ */
+struct transport *raw_udp_transport_create(void);
+
+#endif
diff --git a/sk.c b/sk.c
index f18b2bf..61c4d78 100644
--- a/sk.c
+++ b/sk.c
@@ -439,6 +439,7 @@ int sk_timestamping_init(int fd, const char *device, enum
timestamp_type type,
switch (transport) {
case TRANS_UDP_IPV4:
case TRANS_UDP_IPV6:
+ case TRANS_RAW_UDP_IPV4:
filter2 = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
break;
case TRANS_IEEE_802_3:
diff --git a/transport.c b/transport.c
index 9366fbf..99d47b0 100644
--- a/transport.c
+++ b/transport.c
@@ -25,6 +25,7 @@
#include "udp.h"
#include "udp6.h"
#include "uds.h"
+#include "raw_udp.h"
int transport_close(struct transport *t, struct fdarray *fda)
{
@@ -115,6 +116,9 @@ struct transport *transport_create(struct config *cfg,
case TRANS_IEEE_802_3:
t = raw_transport_create();
break;
+ case TRANS_RAW_UDP_IPV4:
+ t = raw_udp_transport_create();
+ break;
case TRANS_DEVICENET:
case TRANS_CONTROLNET:
case TRANS_PROFINET:
diff --git a/transport.h b/transport.h
index 5b8c413..fb97b12 100644
--- a/transport.h
+++ b/transport.h
@@ -39,6 +39,7 @@ enum transport_type {
TRANS_DEVICENET,
TRANS_CONTROLNET,
TRANS_PROFINET,
+ TRANS_RAW_UDP_IPV4,
};
/**
diff --git a/util.c b/util.c
index 73fb37e..afe496a 100644
--- a/util.c
+++ b/util.c
@@ -192,6 +192,7 @@ int str2addr(enum transport_type type, const char *s,
struct address *addr)
addr->len = sizeof(addr->sin);
break;
case TRANS_IEEE_802_3:
+ case TRANS_RAW_UDP_IPV4:
if (str2mac(s, mac)) {
pr_err("bad Layer-2 address");
return -1;
--
2.8.4
------------------------------------------------------------------------------
Check out the vibrant tech community on one of the world's most
engaging tech sites, Slashdot.org! http://sdm.link/slashdot
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
