Add a transport over raw Ethernet packets.

Signed-off-by: Richard Cochran <[email protected]>

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 sk.o tmtab.o transport.o udp.o util.o
+ print.o raw.o servo.o sk.o tmtab.o transport.o udp.o util.o
 
 OBJECTS	= $(OBJ) phc2sys.o hwstamp_ctl.o
 SRC	= $(OBJECTS:.o=.c)

raw.c

@@ -0,0 +1,259 @@
+/**
+ * @file raw.c
+ * @note Copyright (C) 2012 Richard Cochran <[email protected]>
+ *
+ * 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 <errno.h>
+#include <fcntl.h>
+#include <linux/filter.h>
+#include <linux/if_ether.h>
+#include <net/ethernet.h>
+#include <net/if.h>
+#include <netinet/in.h>
+#include <netpacket/packet.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <linux/errqueue.h>
+#include <linux/net_tstamp.h>
+#include <linux/sockios.h>
+
+#include "contain.h"
+#include "ether.h"
+#include "print.h"
+#include "raw.h"
+#include "sk.h"
+#include "transport_private.h"
+
+struct raw {
+	struct transport t;
+	struct eth_addr addr;
+};
+
+#define OP_AND  (BPF_ALU | BPF_AND | BPF_K)
+#define OP_JEQ  (BPF_JMP | BPF_JEQ | BPF_K)
+#define OP_LDB  (BPF_LD  | BPF_B   | BPF_ABS)
+#define OP_LDH  (BPF_LD  | BPF_H   | BPF_ABS)
+#define OP_RETK (BPF_RET | BPF_K)
+
+#define PTP_GEN_BIT 0x08 /* indicates general message, if set in message type */
+
+#define N_RAW_FILTER    7
+#define RAW_FILTER_TEST 4
+
+static struct sock_filter raw_filter[N_RAW_FILTER] = {
+	{OP_LDH,  0, 0, OFF_ETYPE   },
+	{OP_JEQ,  0, 3, ETH_P_1588  }, /*f goto reject*/
+	{OP_LDB,  0, 0, ETH_HLEN    },
+	{OP_AND,  0, 0, PTP_GEN_BIT }, /*test general bit*/
+	{OP_JEQ,  0, 1, 0           }, /*0,1=accept event; 1,0=accept general*/
+	{OP_RETK, 0, 0, 1500        }, /*accept*/
+	{OP_RETK, 0, 0, 0           }, /*reject*/
+};
+
+static int raw_configure(int fd, int event, int index,
+			 unsigned char *addr, int enable)
+{
+	int filter_test, option;
+	struct packet_mreq mreq;
+	struct sock_fprog prg = { N_RAW_FILTER, raw_filter };
+
+	filter_test = RAW_FILTER_TEST;
+	if (event) {
+		raw_filter[filter_test].jt = 0;
+		raw_filter[filter_test].jf = 1;
+	} else {
+		raw_filter[filter_test].jt = 1;
+		raw_filter[filter_test].jf = 0;
+	}
+
+	if (setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, &prg, sizeof(prg))) {
+		pr_err("setsockopt SO_ATTACH_FILTER failed: %m");
+		return -1;
+	}
+
+	option = enable ? PACKET_ADD_MEMBERSHIP : PACKET_DROP_MEMBERSHIP;
+
+	mreq.mr_ifindex = index;
+	mreq.mr_type = PACKET_MR_MULTICAST;
+	mreq.mr_alen = 6;
+	memcpy(mreq.mr_address, addr, 6);
+	if (!setsockopt(fd, SOL_PACKET, option, &mreq, sizeof(mreq))) {
+		return 0;
+	}
+	pr_warning("setsockopt PACKET_MR_MULTICAST failed: %m");
+
+	mreq.mr_ifindex = index;
+	mreq.mr_type = PACKET_MR_ALLMULTI;
+	mreq.mr_alen = 0;
+	if (!setsockopt(fd, SOL_PACKET, option, &mreq, sizeof(mreq))) {
+		return 0;
+	}
+	pr_warning("setsockopt PACKET_MR_ALLMULTI failed: %m");
+
+	mreq.mr_ifindex = index;
+	mreq.mr_type = PACKET_MR_PROMISC;
+	mreq.mr_alen = 0;
+	if (!setsockopt(fd, SOL_PACKET, option, &mreq, sizeof(mreq))) {
+		return 0;
+	}
+	pr_warning("setsockopt PACKET_MR_PROMISC failed: %m");
+
+	pr_err("all socket options failed");
+	return -1;
+}
+
+static int raw_close(struct transport *t, struct fdarray *fda)
+{
+	close(fda->fd[0]);
+	close(fda->fd[1]);
+	return 0;
+}
+
+static unsigned char ptp_dst_mac[MAC_LEN] = { PTP_DST_MAC };
+
+static int open_socket(char *name, int event)
+{
+	struct sockaddr_ll addr;
+	int fd, index;
+
+	fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
+	if (fd < 0) {
+		pr_err("socket failed: %m");
+		goto no_socket;
+	}
+	index = sk_interface_index(fd, name);
+	if (index < 0)
+		goto no_option;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sll_ifindex = index;
+	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_configure(fd, event, index, ptp_dst_mac, 1))
+		goto no_option;
+
+	return fd;
+no_option:
+	close(fd);
+no_socket:
+	return -1;
+}
+
+static int raw_open(struct transport *t, char *name,
+		    struct fdarray *fda, enum timestamp_type ts_type)
+{
+	struct raw *raw = container_of(t, struct raw, t);
+	int efd, gfd;
+
+	memcpy(raw->addr.dst, ptp_dst_mac, MAC_LEN);
+
+	if (sk_interface_macaddr(name, raw->addr.src, MAC_LEN))
+		goto no_mac;
+
+	efd = open_socket(name, 1);
+	if (efd < 0)
+		goto no_event;
+
+	gfd = open_socket(name, 0);
+	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;
+	fda->cnt = 2;
+	return 0;
+
+no_timestamping:
+	close(gfd);
+no_general:
+	close(efd);
+no_event:
+no_mac:
+	return -1;
+}
+
+static int raw_recv(struct transport *t, int fd, void *buf, int buflen,
+		    struct hw_timestamp *hwts)
+{
+	unsigned char *ptr = buf;
+	ptr    -= sizeof(struct eth_hdr);
+	buflen += sizeof(struct eth_hdr);
+	return sk_receive(fd, ptr, buflen, hwts, 0);
+}
+
+static int raw_send(struct transport *t, struct fdarray *fda, int event,
+		    void *buf, int len, struct hw_timestamp *hwts)
+{
+	struct raw *raw = container_of(t, struct raw, t);
+	ssize_t cnt;
+	int fd = event ? fda->fd[FD_EVENT] : fda->fd[FD_GENERAL];
+	unsigned char pkt[1600], *ptr = buf;
+	struct eth_hdr *hdr;
+
+	ptr -= sizeof(*hdr);
+	len += sizeof(*hdr);
+
+	hdr = (struct eth_hdr *) ptr;
+	memcpy(&hdr->mac, &raw->addr, sizeof(hdr->mac));
+	hdr->type = htons(ETH_P_1588);
+
+	cnt = send(fd, ptr, len, 0);
+	if (cnt < 1) {
+		pr_err("send failed: %d %m", errno);
+		return cnt;
+	}
+	/*
+	 * Get the time stamp right away.
+	 */
+	return event ? sk_receive(fd, pkt, len, hwts, MSG_ERRQUEUE) : cnt;
+}
+
+static void raw_release(struct transport *t)
+{
+	struct raw *raw = container_of(t, struct raw, t);
+	free(raw);
+}
+
+struct transport *raw_transport_create(void)
+{
+	struct raw *raw;
+	raw = calloc(1, sizeof(*raw));
+	if (!raw)
+		return NULL;
+	raw->t.close   = raw_close;
+	raw->t.open    = raw_open;
+	raw->t.recv    = raw_recv;
+	raw->t.send    = raw_send;
+	raw->t.release = raw_release;
+	return &raw->t;
+}

raw.h

@@ -0,0 +1,32 @@
+/**
+ * @file raw.h
+ * @brief Implements transport over IEEE 802.3 aka raw Ethernet.
+ * @note Copyright (C) 2012 Richard Cochran <[email protected]>
+ *
+ * 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_H
+#define HAVE_RAW_H
+
+#include "fd.h"
+#include "transport.h"
+
+/**
+ * Allocate an instance of a raw Ethernet transport.
+ * @return Pointer to a new transport instance on success, NULL otherwise.
+ */
+struct transport *raw_transport_create(void);
+
+#endif

transport.c

@@ -19,6 +19,7 @@
 
 #include "transport.h"
 #include "transport_private.h"
+#include "raw.h"
 #include "udp.h"
 
 int transport_close(struct transport *t, struct fdarray *fda)
@@ -50,7 +51,9 @@ struct transport *transport_create(enum transport_type type)
 	case TRANS_UDP_IPV4:
 		return udp_transport_create();
 	case TRANS_UDP_IPV6:
+		break;
 	case TRANS_IEEE_802_3:
+		return raw_transport_create();
 	case TRANS_DEVICENET:
 	case TRANS_CONTROLNET:
 	case TRANS_PROFINET: