diff --git a/makefile b/makefile index b5301ad..e29754d 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 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) diff --git a/raw.c b/raw.c new file mode 100644 index 0000000..745b1e6 --- /dev/null +++ b/raw.c @@ -0,0 +1,259 @@ +/** + * @file raw.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 +#include +#include +#include + +#include +#include +#include + +#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; +} diff --git a/raw.h b/raw.h new file mode 100644 index 0000000..d062d41 --- /dev/null +++ b/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 + * + * 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 diff --git a/transport.c b/transport.c index 13a5300..be7663f 100644 --- a/transport.c +++ b/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: