1 files changed, 61 insertions, 7 deletions

diff --git a/lib/avtp_pipeline/platform/Linux/rawsock/pcap_rawsock.c b/lib/avtp_pipeline/platform/Linux/rawsock/pcap_rawsock.c
index 1f87c043..de0d7967 100644
--- a/lib/avtp_pipeline/platform/Linux/rawsock/pcap_rawsock.c
+++ b/lib/avtp_pipeline/platform/Linux/rawsock/pcap_rawsock.c
@@ -1,5 +1,6 @@
 /*************************************************************************************************************
 Copyright (c) 2012-2015, Symphony Teleca Corporation, a Harman International Industries, Incorporated company
+Copyright (c) 2016-2017, Harman International Industries, Incorporated
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -38,6 +39,36 @@ https://github.com/benhoyt/inih/commit/74d2ca064fb293bc60a77b0bd068075b293cf175.
 #define	AVB_LOG_COMPONENT	"Raw Socket"
 #include "openavb_log.h"
 
+static pcap_t* open_pcap_dev(const char* ifname, int frameSize, char* errbuf)
+{
+	pcap_t* handle = pcap_create(ifname, errbuf);
+	if (handle) {
+		int err;
+		err = pcap_set_snaplen(handle, frameSize);
+		if (err) AVB_LOGF_WARNING("Cannot set snap len %d", err);
+
+		err = pcap_set_promisc(handle, 1);
+		if (err) AVB_LOGF_WARNING("Cannot set promisc %d", err);
+
+		err = pcap_set_immediate_mode(handle, 1);
+		if (err) AVB_LOGF_WARNING("Cannot set immediate mode %d", err);
+
+		// we need timeout (here 100ms) otherwise we could block for ever
+		err = pcap_set_timeout(handle, 100);
+		if (err) AVB_LOGF_WARNING("Cannot set timeout %d", err);
+
+		err = pcap_set_tstamp_precision(handle, PCAP_TSTAMP_PRECISION_NANO);
+		if (err) AVB_LOGF_WARNING("Cannot set tstamp nano precision %d", err);
+
+		err = pcap_set_tstamp_type(handle, PCAP_TSTAMP_ADAPTER_UNSYNCED);
+		if (err) AVB_LOGF_WARNING("Cannot set tstamp adapter unsynced %d", err);
+
+		err = pcap_activate(handle);
+		if (err) AVB_LOGF_WARNING("Cannot activate pcap %d", err);
+	}
+	return handle;
+}
+
 // Open a rawsock for TX or RX
 void *pcapRawsockOpen(pcap_rawsock_t* rawsock, const char *ifname, bool rx_mode, bool tx_mode, U16 ethertype, U32 frame_size, U32 num_frames)
 {
@@ -67,14 +98,14 @@ void *pcapRawsockOpen(pcap_rawsock_t* rawsock, const char *ifname, bool rx_mode,
 		rawsock->base.frameSize = rawsock->base.ifInfo.mtu + ETH_HLEN + VLAN_HLEN;
 	}
 	else if (rawsock->base.frameSize > rawsock->base.ifInfo.mtu + ETH_HLEN + VLAN_HLEN) {
-		AVB_LOG_ERROR("Creating raswsock; requested frame size exceeds MTU");
+		AVB_LOG_ERROR("Creating rawsock; requested frame size exceeds MTU");
 		free(rawsock);
 		AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK);
 		return NULL;
 	}
 
 	char errbuf[PCAP_ERRBUF_SIZE];
-	rawsock->handle = pcap_open_live(ifname, rawsock->base.frameSize, 1, 1, errbuf);
+	rawsock->handle = open_pcap_dev(ifname, rawsock->base.frameSize, errbuf);
 	if (!rawsock->handle) {
 		AVB_LOGF_ERROR("Cannot open device %s: %s", ifname, errbuf);
 		free(rawsock);
@@ -87,8 +118,10 @@ void *pcapRawsockOpen(pcap_rawsock_t* rawsock, const char *ifname, bool rx_mode,
 	cb->close = pcapRawsockClose;
 	cb->getTxFrame = pcapRawsockGetTxFrame;
 	cb->txFrameReady = pcapRawsockTxFrameReady;
+	cb->send = pcapRawsockSend;
 	cb->getRxFrame = pcapRawsockGetRxFrame;
 	cb->rxMulticast = pcapRawsockRxMulticast;
+	cb->rxParseHdr = pcapRawsockRxParseHdr;
 
 	AVB_TRACE_EXIT(AVB_TRACE_RAWSOCK);
 	return rawsock;
@@ -136,20 +169,28 @@ bool pcapRawsockTxFrameReady(void *pvRawsock, U8 *pBuffer, unsigned int len, U64
 	return ret == 0;
 }
 
+// Send all packets that are ready (i.e. tell kernel to send them)
+int pcapRawsockSend(void *pvRawsock)
+{
+	// pcapRawsock sends frames in pcapRawsockTxFrameReady
+
+	return 1;
+}
+
 U8 *pcapRawsockGetRxFrame(void *pvRawsock, U32 timeout, unsigned int *offset, unsigned int *len)
 {
 	pcap_rawsock_t *rawsock = (pcap_rawsock_t*)pvRawsock;
 
-	struct pcap_pkthdr *header = 0;
+	rawsock->rxHeader = 0;
 	const u_char *packet = 0;
 	int ret;
 
 	if (rawsock) {
-		ret = pcap_next_ex(rawsock->handle, &header, &packet);
+		ret = pcap_next_ex(rawsock->handle, &rawsock->rxHeader, &packet);
 		switch(ret) {
 		case 1:
 			*offset = 0;
-			*len = header->caplen;
+			*len = rawsock->rxHeader->caplen;
 			return (U8*)packet;
 		case -1:
 			AVB_LOGF_ERROR("pcap_next_ex failed: %s", pcap_geterr(rawsock->handle));
@@ -159,7 +200,7 @@ U8 *pcapRawsockGetRxFrame(void *pvRawsock, U32 timeout, unsigned int *offset, un
 			break;
 		case -2:
 			// no packets to be read from savefile
-			// this should not happend
+			// this should not happened
 			break;
 		default:
 			break;
@@ -169,6 +210,19 @@ U8 *pcapRawsockGetRxFrame(void *pvRawsock, U32 timeout, unsigned int *offset, un
 	return NULL;
 }
 
+int pcapRawsockRxParseHdr(void* pvRawsock, U8* pBuffer, hdr_info_t* pInfo)
+{
+	int hdrLen = baseRawsockRxParseHdr(pvRawsock, pBuffer, pInfo);
+
+	pcap_rawsock_t *rawsock = (pcap_rawsock_t*)pvRawsock;
+	if (rawsock && rawsock->rxHeader) {
+		pInfo->ts.tv_sec = rawsock->rxHeader->ts.tv_sec;
+		// we requested nanosecond timestamp precision, so probably we don't have to scale here
+		pInfo->ts.tv_nsec = rawsock->rxHeader->ts.tv_usec;
+	}
+	return hdrLen;
+}
+
 // Setup the rawsock to receive multicast packets
 bool pcapRawsockRxMulticast(void *pvRawsock, bool add_membership, const U8 addr[ETH_ALEN])
 {
@@ -177,7 +231,7 @@ bool pcapRawsockRxMulticast(void *pvRawsock, bool add_membership, const U8 addr[
 	struct bpf_program comp_filter_exp;
 	char filter_exp[30];
 
-	sprintf(filter_exp, "ether dst %x:%x:%x:%x:%x:%x", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+	sprintf(filter_exp, "ether dst %02x:%02x:%02x:%02x:%02x:%02x", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
 
 	AVB_LOGF_DEBUG("%s %d %s", __func__, (int)add_membership, filter_exp);