time-wrs/wrs-socket.c: implementation of net functions for wrs arch

The coding for wr timestamping functions has been mostly copied from ptp-noposix project: file libptpnetif/ptpd_netif.c, commit 2e46712e. Signed-off-by: Aurelio Colosimo <aurelio@aureliocolosimo.it>

time-wrs/wrs-socket.c: implementation of net functions for wrs arch
The coding for wr timestamping functions has been mostly copied from ptp-noposix project: file libptpnetif/ptpd_netif.c, commit 2e46712e. Signed-off-by: Aurelio Colosimo <aurelio@aureliocolosimo.it>
89a46c6d · Aurelio Colosimo · 71c81429 · 89a46c6d
Commit 89a46c6d authored Jul 25, 2013 by Aurelio Colosimo
Hide whitespace changes
Inline Side-by-side

Showing with 403 additions and 14 deletions

wrs-socket.c time-wrs/wrs-socket.c +403 -14

No files found.
--- a/time-wrs/wrs-socket.c
+++ b/time-wrs/wrs-socket.c
@@ -4,30 +4,419 @@

 #include <ppsi/ppsi.h>

-static int wrs_net_init(struct pp_instance *ppi)
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <arpa/inet.h>
+#include <linux/if_packet.h>
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <linux/net_tstamp.h>
+#include <linux/errqueue.h>
+#include <linux/sockios.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <asm/types.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include <ptpdump.h>
+
+#include <ppsi-wrs.h>
+#include <hal_exports.h>
+extern struct minipc_pd __rpcdef_get_port_state;
+
+#define ETHER_MTU 1518
+#define DMTD_UPDATE_INTERVAL 500
+#define PACKED __attribute__((packed))
+
+struct scm_timestamping {
+	struct timespec systime;
+	struct timespec hwtimetrans;
+	struct timespec hwtimeraw;
+};
+
+PACKED struct etherpacket {
+	struct ethhdr ether;
+	char data[ETHER_MTU];
+};
+
+typedef struct
 {
-	/* FIXME */
-	return unix_net_ops.init(ppi);
+	uint64_t start_tics;
+	uint64_t timeout;
+} timeout_t ;
+
+struct wrs_socket {
+	/* parameters for linearization of RX timestamps */
+	uint32_t clock_period;
+	uint32_t phase_transition;
+	uint32_t dmtd_phase;
+	int dmtd_phase_valid;
+	timeout_t dmtd_update_tmo;
+};
+
+static uint64_t get_tics()
+{
+	struct timezone tz = {0, 0};
+	struct timeval tv;
+	gettimeofday(&tv, &tz);
+
+	return (uint64_t) tv.tv_sec * 1000000ULL + (uint64_t) tv.tv_usec;
 }

-static int wrs_net_exit(struct pp_instance *ppi)
+static inline int tmo_init(timeout_t *tmo, uint32_t milliseconds)
 {
-	/* FIXME */
-	return unix_net_ops.exit(ppi);
+	tmo->start_tics = get_tics();
+	tmo->timeout = (uint64_t) milliseconds * 1000ULL;
+	return 0;
 }

-static int wrs_net_recv(struct pp_instance *ppi, void *pkt, int len,
-			TimeInternal *t)
+static inline int tmo_restart(timeout_t *tmo)
 {
-	/* FIXME */
-	return unix_net_ops.recv(ppi, pkt, len, t);
+	tmo->start_tics = get_tics();
+	return 0;
 }

-static int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
-			TimeInternal *t, int chtype, int use_pdelay_addr)
+static inline int tmo_expired(timeout_t *tmo)
+{
+	return (get_tics() - tmo->start_tics > tmo->timeout);
+}
+
+/* checks if x is inside range <min, max> */
+static inline int inside_range(int min, int max, int x)
+{
+	if(min < max)
+		return (x>=min && x<=max);
+	else
+		return (x<=max || x>=min);
+}
+
+static void update_dmtd(struct wrs_socket *s, char *ifname)
+{
+	hexp_port_state_t pstate;
+
+	if (tmo_expired(&s->dmtd_update_tmo))
+	{
+		minipc_call(hal_ch, DEFAULT_TO, &__rpcdef_get_port_state,
+			 &pstate, ifname);
+
+		s->dmtd_phase = pstate.phase_val;
+		s->dmtd_phase_valid = pstate.phase_val_valid;
+
+		tmo_restart(&s->dmtd_update_tmo);
+	}
+}
+
+static void wrs_linearize_rx_timestamp(TimeInternal *ts,
+	int32_t dmtd_phase, int cntr_ahead, int transition_point,
+	int clock_period)
+{
+	int trip_lo, trip_hi;
+	int phase;
+
+	phase = clock_period -1 -dmtd_phase;
+
+	/* calculate the range within which falling edge timestamp is stable
+	 * (no possible transitions) */
+	trip_lo = transition_point - clock_period / 4;
+	if(trip_lo < 0) trip_lo += clock_period;
+
+	trip_hi = transition_point + clock_period / 4;
+	if(trip_hi >= clock_period) trip_hi -= clock_period;
+
+	if(inside_range(trip_lo, trip_hi, phase))
+	{
+		/* We are within +- 25% range of transition area of
+		 * rising counter. Take the falling edge counter value as the
+		 * "reliable" one. cntr_ahead will be 1 when the rising edge
+		 * counter is 1 tick ahead of the falling edge counter */
+
+		ts->nanoseconds -= cntr_ahead ? (clock_period / 1000) : 0;
+
+		/* check if the phase is before the counter transition value
+		 * and eventually increase the counter by 1 to simulate a
+		 * timestamp transition exactly at s->phase_transition
+		 * DMTD phase value */
+		if(inside_range(trip_lo, transition_point, phase))
+			ts->nanoseconds += clock_period / 1000;
+
+	}
+
+	ts->phase = phase - transition_point - 1;
+	if(ts->phase  < 0) ts->phase += clock_period;
+	ts->phase = clock_period - 1 -ts->phase;
+}
+
+
+int wrs_recv_msg(struct pp_instance *ppi, int fd, void *pkt, int _len,
+			  TimeInternal *t)
+{
+	struct wrs_socket *s;
+	struct msghdr msg;
+	struct iovec entry;
+	struct sockaddr_ll from_addr;
+	struct {
+		struct cmsghdr cm;
+		char control[1024];
+	} control;
+	struct cmsghdr *cmsg;
+	struct scm_timestamping *sts = NULL;
+
+	s = (struct wrs_socket*)NP(ppi)->ch[PP_NP_GEN].arch_data;
+	size_t len = _len + sizeof(struct ethhdr);
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_iov = &entry;
+	msg.msg_iovlen = 1;
+	entry.iov_base = pkt;
+	entry.iov_len = len;
+	msg.msg_name = (caddr_t)&from_addr;
+	msg.msg_namelen = sizeof(from_addr);
+	msg.msg_control = &control;
+	msg.msg_controllen = sizeof(control);
+
+	int ret = recvmsg(fd, &msg, MSG_DONTWAIT);
+
+	if (ret < 0 && errno == EAGAIN) return 0; // would be blocking
+
+	if (ret == -EAGAIN) return 0;
+
+	if (ret <= 0) return ret;
+
+	/* FIXME Check ptp-noposix, commit d34f56f: if sender mac check
+	 * is required. Should be added here */
+
+	if (t)
+		t->correct = 0;
+
+	for (cmsg = CMSG_FIRSTHDR(&msg);
+	     cmsg;
+	     cmsg = CMSG_NXTHDR(&msg, cmsg)) {
+
+		void *dp = CMSG_DATA(cmsg);
+
+		if(cmsg->cmsg_level == SOL_SOCKET
+		   && cmsg->cmsg_type == SO_TIMESTAMPING)
+			sts = (struct scm_timestamping *) dp;
+
+	}
+
+	if(sts && t)
+	{
+		int cntr_ahead = sts->hwtimeraw.tv_sec & 0x80000000 ? 1: 0;
+		t->nanoseconds = sts->hwtimeraw.tv_nsec;
+		t->seconds =
+			(uint64_t) sts->hwtimeraw.tv_sec & 0x7fffffff;
+
+		t->raw_ahead = cntr_ahead;
+
+		update_dmtd(s, ppi->iface_name);
+		if (s->dmtd_phase_valid)
+		{
+			wrs_linearize_rx_timestamp(t, s->dmtd_phase,
+				cntr_ahead, s->phase_transition, s->clock_period);
+			t->correct = 1;
+		}
+	}
+
+	return ret;
+}
+
+int wrs_net_recv(struct pp_instance *ppi, void *pkt, int len,
+		   TimeInternal *t)
+{
+	struct pp_channel *ch1, *ch2;
+	int ret;
+
+	if (ppi->ethernet_mode) {
+		int fd = NP(ppi)->ch[PP_NP_GEN].fd;
+
+		ret = wrs_recv_msg(ppi, fd, pkt, len, t);
+		if (ret > 0 && pp_diag_allow(ppi, frames, 2))
+			dump_1588pkt("recv: ", pkt, ret, t);
+		return ret;
+	}
+
+	pp_error("udp is not supported in wrs arch yet\n");
+	return -ENOTSUP;
+}
+
+/* Waits for the transmission timestamp and stores it in t (if not null). */
+static void poll_tx_timestamp(struct wrs_socket *s, int fd,
+							  TimeInternal *t)
+{
+	char data[16384];
+
+	struct msghdr msg;
+	struct iovec entry;
+	struct sockaddr_ll from_addr;
+	struct {
+		struct cmsghdr cm;
+		char control[1024];
+	} control;
+	struct cmsghdr *cmsg;
+	int res;
+	uint32_t rtag;
+
+	struct sock_extended_err *serr = NULL;
+	struct scm_timestamping *sts = NULL;
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_iov = &entry;
+	msg.msg_iovlen = 1;
+	entry.iov_base = data;
+	entry.iov_len = sizeof(data);
+	msg.msg_name = (caddr_t)&from_addr;
+	msg.msg_namelen = sizeof(from_addr);
+	msg.msg_control = &control;
+	msg.msg_controllen = sizeof(control);
+
+	res = recvmsg(fd, &msg, MSG_ERRQUEUE);
+
+	if (t)
+		t->correct = 0;
+
+	if (res <= 0)
+		return;
+
+	memcpy(&rtag, data + res - 4, 4);
+
+	for (cmsg = CMSG_FIRSTHDR(&msg);
+	     cmsg;
+	     cmsg = CMSG_NXTHDR(&msg, cmsg)) {
+
+			void *dp = CMSG_DATA(cmsg);
+
+			if(cmsg->cmsg_level == SOL_PACKET
+			   && cmsg->cmsg_type == PACKET_TX_TIMESTAMP)
+				serr = (struct sock_extended_err *) dp;
+
+			if(cmsg->cmsg_level == SOL_SOCKET
+			   && cmsg->cmsg_type == SO_TIMESTAMPING)
+				sts = (struct scm_timestamping *) dp;
+
+			if(serr && sts && t)
+			{
+				t->correct = 1;
+				t->phase = 0;
+				t->nanoseconds = sts->hwtimeraw.tv_nsec;
+				t->seconds = (uint64_t) sts->hwtimeraw.tv_sec & 0x7fffffff;
+			}
+	}
+}
+
+int wrs_net_send(struct pp_instance *ppi, void *pkt, int len,
+			  TimeInternal *t, int chtype, int use_pdelay_addr)
+{
+	struct sockaddr_in addr;
+	struct ethhdr *hdr = pkt;
+	struct wrs_socket *s;
+	int ret;
+
+	s = (struct wrs_socket *)NP(ppi)->ch[PP_NP_GEN].arch_data;
+
+	if (ppi->ethernet_mode) {
+		int fd = NP(ppi)->ch[PP_NP_GEN].fd;
+		hdr->h_proto = htons(ETH_P_1588);
+
+		memcpy(hdr->h_dest, PP_MCAST_MACADDRESS, ETH_ALEN);
+		/* raw socket implementation always uses gen socket */
+		memcpy(hdr->h_source, NP(ppi)->ch[PP_NP_GEN].addr, ETH_ALEN);
+
+		if (t)
+			ppi->t_ops->get(ppi, t);
+
+		ret = send(fd, hdr, len, 0);
+		if (pp_diag_allow(ppi, frames, 2))
+			dump_1588pkt("send: ", pkt, len, t);
+
+		poll_tx_timestamp(s, fd, t);
+
+		return ret;
+	}
+
+	pp_error("udp is not supported in wrs arch yet\n");
+	return -ENOTSUP;
+
+}
+
+static int wrs_net_init(struct pp_instance *ppi)
+{
+	int r, i;
+	hexp_port_state_t pstate;
+
+	r = unix_net_ops.init(ppi);
+	if (r)
+		return r;
+
+	r = minipc_call(hal_ch, DEFAULT_TO, &__rpcdef_get_port_state,
+		 &pstate, ppi->iface_name);
+
+	if (r)
+		return r;
+
+	/* Only one wrs_socket is created for each pp_instance, because
+	 * wrs_socket is related to the interface and not to the pp_channel */
+	struct wrs_socket *s = calloc(0, sizeof(struct wrs_socket));
+
+	if (!s)
+		return -1;
+
+	s->clock_period = pstate.clock_period;
+	s->phase_transition = pstate.t2_phase_transition;
+	s->dmtd_phase_valid = 0;
+	s->dmtd_phase = pstate.phase_val;
+
+	NP(ppi)->ch[PP_NP_GEN].arch_data = s;
+	NP(ppi)->ch[PP_NP_EVT].arch_data = s;
+	tmo_init(&s->dmtd_update_tmo, DMTD_UPDATE_INTERVAL);
+
+	/* Hw timestamping enable */
+	int so_timestamping_flags = SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RAW_HARDWARE;
+
+	struct ifreq ifr;
+	struct hwtstamp_config hwconfig;
+
+	strncpy(ifr.ifr_name, ppi->iface_name, sizeof(ifr.ifr_name));
+
+	hwconfig.tx_type = HWTSTAMP_TX_ON;
+	hwconfig.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
+
+	ifr.ifr_data = &hwconfig;
+
+	for (i = PP_NP_GEN; i <= PP_NP_EVT; i++) {
+		int fd = NP(ppi)->ch[i].fd;
+
+		if (fd <= 0)
+			break;
+
+		if (ioctl(fd, SIOCSHWTSTAMP, &ifr) < 0) {
+			perror("SIOCSHWTSTAMP");
+			return -1;
+		}
+
+		if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,
+			&so_timestamping_flags, sizeof(int)) < 0) {
+			pp_error("setsockopt(SO_TIMESTAMPING)");
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+static int wrs_net_exit(struct pp_instance *ppi)
 {
-	/* FIXME */
-	return unix_net_ops.send(ppi, pkt, len, t, chtype, use_pdelay_addr);
+	unix_net_ops.exit(ppi);
+	free(NP(ppi)->ch[PP_NP_GEN].arch_data);
+	return 0;
 }

 struct pp_network_operations wrs_net_ops = {