Commit 1dadb834 authored by garciay's avatar garciay
Browse files

Add support of offline pcap capture

Add CommsigniaLayer.receive() method
parent 6ec3bb99
......@@ -29,7 +29,7 @@ import org.jnetpcap.PcapHeader;
import org.jnetpcap.PcapIf;
public class PcapMultiplexer implements Runnable {
/**
* Unique instance of the factory
*/
......@@ -38,41 +38,54 @@ public class PcapMultiplexer implements Runnable {
private static byte[] MAC_BROADCAST = new byte[]{(byte)0xFF,(byte)0xFF,(byte)0xFF,(byte)0xFF,(byte)0xFF,(byte)0xFF};
private StringBuilder errbuf = new StringBuilder(); // For any error msgs
private long timestampOffset = 0;
private PcapMultiplexer() {
filter = "";
// Obtain the list of network interfaces
List<PcapIf> alldevs = new ArrayList<PcapIf>(); // Will be filled with NICs
int r = Pcap.findAllDevs(alldevs, errbuf);
if (r == Pcap.NOT_OK || alldevs.isEmpty()) {
//TERFactory.getInstance().logError("Can't read list of devices, error is %s" + errbuf.toString());
return;
}
// Find the right interface
int ifaceIndex = 0;
String expectedIface = ((CharstringValue)TERFactory.getInstance().getTaParameter("LocalEthernetMAC")).getString().toLowerCase();
for( ; ifaceIndex < alldevs.size(); ifaceIndex++) {
try {
if (expectedIface.equalsIgnoreCase(ByteHelper.byteArrayToString(alldevs.get(ifaceIndex).getHardwareAddress()))) {
// Interface found
break;
offlineMode = ((CharstringValue)TERFactory.getInstance().getTaParameter("OfflineMode")).getString().toLowerCase().equals("true");
if (!offlineMode) {
// Obtain the list of network interfaces
List<PcapIf> alldevs = new ArrayList<PcapIf>(); // Will be filled with NICs
int r = Pcap.findAllDevs(alldevs, errbuf);
if (r == Pcap.NOT_OK || alldevs.isEmpty()) {
TERFactory.getInstance().logError("Can't read list of devices, error is %s" + errbuf.toString());
return;
}
// Find the right interface
int ifaceIndex = 0;
String expectedIface = ((CharstringValue)TERFactory.getInstance().getTaParameter("LocalEthernetMAC")).getString().toLowerCase();
for( ; ifaceIndex < alldevs.size(); ifaceIndex++) {
try {
if (expectedIface.equalsIgnoreCase(ByteHelper.byteArrayToString(alldevs.get(ifaceIndex).getHardwareAddress()))) {
// Interface found
break;
}
} catch (IOException e) {
// ignore
}
} catch (IOException e) {
// ignore
}
// Check result
if (ifaceIndex == alldevs.size()) {
throw new RuntimeException(String.format("PcapMultiplexer: Network interface %s not found", expectedIface));
}
device = alldevs.get(ifaceIndex);
//TERFactory.getInstance().logDebug("Listening: " + device.getName());
} else {
file = ((CharstringValue)TERFactory.getInstance().getTaParameter("PcapFile")).getString().toLowerCase();
if ((file == null) || file.isEmpty()) {
throw new RuntimeException(String.format("PcapMultiplexer: failed to open '%s'", file));
}
String tmp = ((CharstringValue)TERFactory.getInstance().getTaParameter("OffsetTime")).getString().toLowerCase();
if (!tmp.isEmpty()) {
timestampOffset = Long.parseLong(tmp);
}
}
// Check result
if (ifaceIndex == alldevs.size()) {
throw new RuntimeException(String.format("EthernetLayer.register: Network interface %s not found", expectedIface));
}
device = alldevs.get(ifaceIndex);
//TERFactory.getInstance().logDebug("Listening: " + device.getName());
}
/**
......@@ -87,18 +100,34 @@ public class PcapMultiplexer implements Runnable {
//TERFactory.getInstance().logDebug(">>>PcapMultiplexer.registering: " + frameType);
if(clientsToMacs.isEmpty()) {
// Open interface
int snaplen = 64 * 1024; // Capture all packets, no truncation
int flags = Pcap.MODE_PROMISCUOUS; // capture all packets
int timeout = 10; // 10 millis
pcap = Pcap.openLive(device.getName(), snaplen, flags, timeout, errbuf);
if (pcap == null) {
//TERFactory.getInstance().logError("Error while opening device for capture: " + errbuf.toString());
return;
if (!offlineMode) { // Open interface
int snaplen = 64 * 1024; // Capture all packets, no truncation
int flags = Pcap.MODE_PROMISCUOUS; // capture all packets
int timeout = 10; // 10 millis
pcap = Pcap.openLive(device.getName(), snaplen, flags, timeout, errbuf);
if (pcap == null) { // Check result
TERFactory.getInstance().logError("Error while opening device for capture: " + errbuf.toString());
return;
}
captureThread = new Thread(this);
captureThread.start();
filter = "";
} else { // Open file
pcap = Pcap.openOffline(file, errbuf);
if (pcap == null) { // Check result
TERFactory.getInstance().logError("Error while opening device for capture: " + errbuf.toString());
return;
}
captureThread = new Thread(this);
captureThread.start();
filter = "";
}
if (pcap == null) { // Check result
TERFactory.getInstance().logError("Error while opening device for capture: " + errbuf.toString());
return;
}
captureThread = new Thread(this);
captureThread.start();
captureThread.start();
filter = "";
}
else {
......@@ -157,12 +186,25 @@ public class PcapMultiplexer implements Runnable {
public void run() {
ByteBufferHandler<Object> handler = new ByteBufferHandler<Object>() {
private boolean couldStart = false;
@Override
public void nextPacket(PcapHeader pcapHeader, ByteBuffer byteBuffer, Object user) {
if(byteBuffer.remaining() < 14) {
return;
}
if (!couldStart) {
if (offlineMode) {
if (pcapHeader.timestampInMillis() < timestampOffset) {
return; // Skip it
} else {
couldStart = true;
}
} else {
couldStart = true;
}
} // continue
Map<String, Object> lowerInfo = new HashMap<String, Object>();
......@@ -198,6 +240,14 @@ public class PcapMultiplexer implements Runnable {
}
};
if (offlineMode) {
try {
Thread.sleep(4000); // TOTO Use a parameter instead of an hardcoded value
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
pcap.loop(-1, handler, null);
}
......@@ -217,6 +267,26 @@ public class PcapMultiplexer implements Runnable {
return null;
}
public void resetFilter(String pcapFilter) {
// Sanity check
if ((pcapFilter == null) || pcapFilter.isEmpty()) {
return;
}
filter = pcapFilter;
TERFactory.getInstance().logDebug("resetFilter: New filter: " + filter);
// Apply filter
PcapBpfProgram bpfFilter = new PcapBpfProgram();
int optimize = 0; // 1 means true, 0 means false
int netmask = 0;
int r = pcap.compile(bpfFilter, filter, optimize, netmask);
if (r != Pcap.OK) {
TERFactory.getInstance().logError("Filter error: " + pcap.getErr());
}
pcap.setFilter(bpfFilter);
}
/**
* Jpcap capture device
*/
......@@ -227,9 +297,12 @@ public class PcapMultiplexer implements Runnable {
*/
private Thread captureThread;
PcapIf device;
PcapIf device = null;
boolean offlineMode = false;
String file = "";
private String filter;
private Map<String, byte[]> clientsToMacs = new HashMap<String, byte[]>();
private Map<String, Short> clientsToFrameTypes = new HashMap<String, Short>();
private HashMap<String, Layer> clientsToLayers = new HashMap<String, Layer>();
}
\ No newline at end of file
......@@ -3,6 +3,7 @@ package org.etsi.its.adapter.layers;
import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Method;
import java.nio.ByteBuffer;
import java.util.Arrays;
import java.util.HashMap;
import java.util.Map;
import java.util.Stack;
......@@ -11,28 +12,27 @@ import java.util.concurrent.TimeoutException;
import org.etsi.adapter.TERFactory;
import org.etsi.common.ByteHelper;
import org.etsi.its.adapter.IManagementLayers;
import org.etsi.its.adapter.PcapMultiplexer;
import com.commsignia.v2x.client.ITSApplication;
import com.commsignia.v2x.client.ITSEventAdapter;
import com.commsignia.v2x.client.MessageSet;
import com.commsignia.v2x.client.exception.ClientException;
import com.commsignia.v2x.client.model.BTPType;
import com.commsignia.v2x.client.model.GNNotification;
import com.commsignia.v2x.client.model.GeonetSendData;
import com.commsignia.v2x.client.model.InjectData;
import com.commsignia.v2x.client.model.InjectData.Builder;
import com.commsignia.v2x.client.model.InjectData.Type;
import com.commsignia.v2x.client.model.WsmpNotification;
import com.commsignia.v2x.client.model.WsmpSendData;
import com.commsignia.v2x.client.model.dev.DeviceId;
import com.commsignia.v2x.client.model.dev.DeviceInfoResponse;
import com.commsignia.v2x.client.model.dev.FacilityModule;
import rx.Observable;
import rx.Scheduler;
import rx.Scheduler.Worker;
public class CommsigniaLayer extends Layer {
public class CommsigniaLayer extends Layer implements IEthernetSpecific {
private static final byte[] DeviceMacAddress = new byte[] { (byte)0x70, (byte)0xb3, (byte)0xd5, (byte)0xf2, (byte)0xa1, (byte)0xe3 };
private static final String TargetHost = "172.17.15.38";
private static final int TargetPort = 7942;
private static final int SourcePort = 7943;
private static final int ItsAid = 5;
private static final int TxPowerDbm = -10; // Max value: -33dBm, RSU: -30dBm, Lab: -10dBm
private static String pcapFilter = null;
/**
* Well-known Ethernet broadcast address
......@@ -44,6 +44,10 @@ public class CommsigniaLayer extends Layer {
*/
public static final String LINK_LAYER_DESTINATION = "LinkLayerDestination";
private Map<String, byte[]> ClientsToMacs = new HashMap<String, byte[]>();
private Map<String, Short> ClientsToFrameTypes = new HashMap<String, Short>();
private HashMap<String, Layer> ClientsToLayers = new HashMap<String, Layer>();
/**
* Constructor
* @param management Layer management instance
......@@ -52,11 +56,8 @@ public class CommsigniaLayer extends Layer {
public CommsigniaLayer(IManagementLayers management, Stack<String> lowerStack) {
super(management, lowerStack);
int its_aid = 5;
String targetHost = "172.17.15.38";
int targetPort = 7942;
MessageSet defaultMessageSet = MessageSet.C;
itsApplication = new ITSApplication(its_aid, targetHost, targetPort, defaultMessageSet);
itsApplication = new ITSApplication(ItsAid, TargetHost, TargetPort, defaultMessageSet);
try {
itsApplication.connect(1000);
itsApplication.registerBlocking();
......@@ -64,18 +65,18 @@ public class CommsigniaLayer extends Layer {
System.out.println(deviceInfoResponse);
itsApplication.setFacilityModuleStatus(FacilityModule.BSM, false);
itsApplication.setFacilityModuleStatus(FacilityModule.CAM, false);
itsApplication.addEventListener(new ITSEventAdapter() {
@Override
public void onGnNotification(GNNotification notification) {
ByteBuffer buffer = ByteBuffer.wrap(notification.getData());
System.out.printf("GN GBC receive. GN address: %s Sequence number: %d RSSI: %d dBm\n",
notification.getGNAddress(),
buffer.getInt(),
notification.getRssi()
);
}
});
// itsApplication.addEventListener(new ITSEventAdapter() {
// @Override
// public void onGnNotification(GNNotification notification) {
// ByteBuffer buffer = ByteBuffer.wrap(notification.getData());
//
// System.out.printf("GN GBC receive. GN address: %s Sequence number: %d RSSI: %d dBm\n",
// notification.getGNAddress(),
// buffer.getInt(),
// notification.getRssi()
// );
// }
// });
itsApplication. gnBindBlocking(BTPType.NONE, 65535);
} catch (TimeoutException e) {
e.printStackTrace();
......@@ -91,6 +92,7 @@ public class CommsigniaLayer extends Layer {
*/
@Override
public void register(Layer upperLayer) {
TERFactory.getInstance().logDebug(">>> CommsigniaLayer.register: " + upperLayer);
if(registeredUpperLayer == null) {
super.register(upperLayer);
......@@ -113,6 +115,77 @@ public class CommsigniaLayer extends Layer {
}
localMacAddress = management.getLinkLayerAddress();
PcapMultiplexer.getInstance().register(this, DeviceMacAddress, this.getEthernetType());
}
if(ClientsToMacs.isEmpty()) {
pcapFilter = "udp dst port " + SourcePort + " and ";
} else {
TERFactory.getInstance().logDebug("CommsigniaLayer.register: Another Client !");
pcapFilter = pcapFilter + " and ";
}
// Update Filter
String strMacAddress = String.format("%02x", localMacAddress[0]);
for(int i=1; i < localMacAddress.length; i++) {
strMacAddress += String.format(":%02x", localMacAddress[i]);
}
//udp dst port 74 && wlan src 8b:ad:f0:0d:01:02
pcapFilter = pcapFilter + "not wlan src " + "8b:ad:f0:0d:01:03";//strMacAddress;
// Reset filter
PcapMultiplexer.getInstance().resetFilter(pcapFilter);
// Register client
ClientsToMacs.put(this.toString(), localMacAddress);
ClientsToLayers.put(this.toString(), upperLayer);
ClientsToFrameTypes.put(this.toString(), upperLayerFrameType);
}
/* (non-Javadoc)
* @see org.etsi.its.adapter.layers.Layer#receive(byte[])
*/
@Override
public void receive(byte[] message, Map<String, Object> lowerInfo) {
TERFactory.getInstance().logDebug(">>> CommsigniaLayer.receive: " + ByteHelper.byteArrayToString(message));
ByteBuffer byteBuffer = ByteBuffer.wrap(message);
// Skip C2P protocol
byteBuffer.position(
20 + // IP Layer: 45 00 01 1f 13 8c 00 00 80 11 6b 0b ac 11 0f 26 ff ff ff ff
8 + // UDP Layer: 75 30 1f 07 01 0b a6 cd
29 + // C2P Layer: 12 00 00 33 41 00 00 03 5c ac 00 02 0c 02 35 a4 e9 01 6b 49 d2 01 3f ff 00 00 7f ff 16
4 // IEEE 802.11L Layer: 88 00 00 00 00
);
// Extract Dst
byte[] dst = new byte[6];
byteBuffer.get(dst, 0, dst.length);
lowerInfo.put(EthernetLayer.LINK_LAYER_DESTINATION, dst);
// Skip Src
byteBuffer.position(byteBuffer.position() + 6);
// Skip IEEE 802.11L Layer
byteBuffer.position(byteBuffer.position() + 10);
// Skip LLC header
byteBuffer.position(byteBuffer.position() + 6);
// Extract FrameType info
byte[] rawFrameType = new byte[2];
byteBuffer.get(rawFrameType, 0, rawFrameType.length);
short frameType = ByteHelper.byteArrayToInt(rawFrameType).shortValue();
// Extract Data
byte[] data = new byte[byteBuffer.remaining()];
byteBuffer.get(data, 0, byteBuffer.remaining());
// Dispatch
for (String mapKey : ClientsToMacs.keySet()) {
if(frameType == ClientsToFrameTypes.get(mapKey)) {
if(Arrays.equals(dst, MAC_BROADCAST) || Arrays.equals(dst, ClientsToMacs.get(mapKey))) {
ClientsToLayers.get(mapKey).receive(data, lowerInfo);
}
}
}
}
......@@ -122,7 +195,7 @@ public class CommsigniaLayer extends Layer {
*/
@Override
public boolean send(byte[] message, Map<String, Object> params) {
TERFactory.getInstance().logDebug(">>> CommsigniaLayer.send: " + ByteHelper.byteArrayToString(message));
//TERFactory.getInstance().logDebug(">>> CommsigniaLayer.send: " + ByteHelper.byteArrayToString(message));
byte[] dst = (byte[])params.get(LINK_LAYER_DESTINATION);
if(dst == null) {
......@@ -159,13 +232,14 @@ public class CommsigniaLayer extends Layer {
.withSrcAddress(srcAddress)
.withType(Type.GNP)
.withInterfaceID(2)
.withTxPowerDbm(TxPowerDbm)
.withData(message);
InjectData injectData = build.build();
TERFactory.getInstance().logDebug("CommsigniaLayer.send: " + ByteHelper.byteArrayToString(injectData.getData()));
//TERFactory.getInstance().logDebug("CommsigniaLayer.send: " + ByteHelper.byteArrayToString(injectData.getData()));
itsApplication.sendOnRadioBlocking(injectData);
TERFactory.getInstance().logDebug("<<< GnLayer.send: " + ByteHelper.byteArrayToString(packet));
return super.send(packet, params);
//TERFactory.getInstance().logDebug("<<< CommsigniaLayer.send: " + ByteHelper.byteArrayToString(packet));
return true;//super.send(packet, params);
} catch (ClientException e) {
e.printStackTrace();
} catch (Exception ex) {
......@@ -180,19 +254,36 @@ public class CommsigniaLayer extends Layer {
*/
@Override
public void unregister(Layer upperLayer) {
if (itsApplication != null) {
try {
itsApplication.gnCloseBlocking(BTPType.NONE, 65535);
itsApplication.deregisterBlocking();
} catch (ClientException e) {
// TODO Auto-generated catch block
e.printStackTrace();
PcapMultiplexer.getInstance().unregister(this);
if(ClientsToMacs.containsKey(this.toString())) {
ClientsToMacs.remove(this.toString());
ClientsToFrameTypes.remove(this.toString());
ClientsToLayers.remove(this.toString());
if(ClientsToMacs.isEmpty()) {
if (itsApplication != null) {
try {
itsApplication.gnCloseBlocking(BTPType.NONE, 65535);
itsApplication.deregisterBlocking();
} catch (ClientException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
itsApplication.shutdown();
itsApplication = null;
}
}
itsApplication.shutdown();
itsApplication = null;
}
}
/* (non-Javadoc)
* @see org.etsi.its.adapter.ports.IEthernetSpecific#getEthernetType()
*/
@Override
public short getEthernetType() {
return (short)0x0800;
}
/**
* Local Ethernet address
*/
......
......@@ -64,6 +64,8 @@ public class UpperTesterPort extends AdapterPort implements IPort, IObservable {
private ByteArrayOutputStream _hashedId8;
private ByteArrayOutputStream _signingPrivateKey;
private String _offlineMode = "";
/**
* Constructor
......@@ -79,6 +81,7 @@ public class UpperTesterPort extends AdapterPort implements IPort, IObservable {
_utSecuredMode = ((CharstringValue)TERFactory.getInstance().getTaParameter("UtSecuredMode")).getString();
_utSecuredRootPath = ((CharstringValue)TERFactory.getInstance().getTaParameter("TsSecuredRootPath")).getString();
_utSecuredConfiId = ((CharstringValue)TERFactory.getInstance().getTaParameter("TsSecuredConfiId")).getString();
_offlineMode = ((CharstringValue)TERFactory.getInstance().getTaParameter("OfflineMode")).getString().toLowerCase();
String settings = ((CharstringValue)TERFactory.getInstance().getTaParameter("UpperTesterSettings")).getString();
Matcher matcher = settingsPattern.matcher(settings);
if (matcher.find()) {
......@@ -122,82 +125,84 @@ public class UpperTesterPort extends AdapterPort implements IPort, IObservable {
@Override
public boolean send(final byte[] message) {
/* FIXME: For debug only. Uncomment if no UT
try {
Thread.sleep(500);
} catch (InterruptedException e) {
// Do nothing, we do not care
}
setChanged();
byte[] rsp;
switch (message[0]) {
case (byte)0x00:
rsp = new byte[]{(byte)0x01, (byte)0x01};
break;
case (byte)0x02: // UtChangePosition
rsp = new byte[]{(byte)0x03, (byte)0x01};
break;
case (byte)0x04: // UtChangePseudonym
rsp = new byte[]{(byte)0x05, (byte)0x01};
break;
case (byte)0x06: // UtActivatePositionTime
rsp = new byte[]{(byte)0x07, (byte)0x01};
break;
case (byte)0x08: // UtDeactivatePositionTime
rsp = new byte[]{(byte)0x09, (byte)0x01};
break;
case (byte)0x10: // UtDenmTrigger
rsp = ByteHelper.concat(
new byte[]{ (byte)0x11, (byte)0x01 },
new byte[] { (byte)0xAA, (byte)0xAA, (byte)0xAA, (byte)0xAA }, // StationID
new byte[] { (byte)0x01, (byte)0xFF } // SequenceNumber
);
break;
case (byte)0x12: // UtDenmUpdate
rsp = ByteHelper.concat(
new byte[]{ (byte)0x13, (byte)0x01 },
new byte[] { (byte)0xAA, (byte)0xAA, (byte)0xAA, (byte)0xAA }, // StationID
new byte[] { (byte)0x01, (byte)0xFF } // SequenceNumber
);
break;
case (byte)0x14: // UtDenmTermination
rsp = new byte[]{(byte)0x15, (byte)0x01};
break;
//reserved(0x92),
case (byte)0x93: // UtSec_setCertificate
case (byte)0x94: // UtSec_setPrivateKey
case (byte)0x95: // UtSec_setTrustPoint(
rsp = new byte[]{(byte)0x91, (byte)0x01}; // UtSecResult
break;
case (byte)0xA0: // UtMapemSpatemTrigger
rsp = new byte[]{(byte)0xA1, (byte)0x01}; // UtMapemSpatemTriggerResult
break;
case (byte)0xA4: // UtIvimTrigger
rsp = new byte[]{(byte)0xA5, (byte)0x01, (byte)0xA0, (byte)0xA0}; // UtIvimTriggerResult
break;
case (byte)0xA6: // UtIvimUpdate
rsp = new byte[]{(byte)0xA7, (byte)0x01, (byte)0xA0, (byte)0xA0}; // UtIvimUpdateResult
break;
case (byte)0xA8: // UtIvimTermination
rsp = new byte[]{(byte)0xA9, (byte)0x01, (byte)0xA0, (byte)0xA0}; // UtIvimTerminationResult
break;
case (byte)0xAB: // UtSremTrigger
rsp = new byte[]{(byte)0xAC, (byte)0x01}; // UtSremTriggerResult
break;
case (byte)0xAD: // UtSremUpdate
rsp = new byte[]{(byte)0xAE, (byte)0x01}; // UtSremUpdateResult
break;
default:
if ((message[0] >= (byte)0x30) && (message[0] <= (byte)0x3F)) { // UtCamTrigger_xxx
rsp = new byte[]{(byte)0x21, (byte)0x00}; // UtCamTriggerResult
} else {
rsp = new byte[]{(byte)0x24, (byte)0x00};
}
break;
if (_offlineMode.equals("true")) {
/* FIXME: For debug only. Uncomment if no UT*/
try {
Thread.sleep(500);
} catch (InterruptedException e) {
// Do nothing, we do not care
}
setChanged();
byte[] rsp;
switch (message[0]) {
case (byte)0x00:
rsp = new byte[]{(byte)0x01, (byte)0x01};
break;
case (byte)0x02: // UtChangePosition
rsp = new byte[]{(byte)0x03, (byte)0x01};
break;
case (byte)0x04: // UtChangePseudonym
rsp = new byte[]{(byte)0x05, (byte)0x01};
break;
case (byte)0x06: // UtActivatePositionTime
rsp = new byte[]{(byte)0x07, (byte)0x01};