CommsigniaLayer.java

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;
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.MessageSet;
import com.commsignia.v2x.client.exception.ClientException;
import com.commsignia.v2x.client.model.BTPType;
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.dev.DeviceInfoResponse;
import com.commsignia.v2x.client.model.dev.FacilityModule;

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 = "10.200.1.101";
    private static final int TargetPort = 7942; 
    private static final int SourcePort = 7943; 
    private static final int ItsAid = 5; 
    private static final int InterfaceID = 2;
    private static final int TxPowerDbm = -32; // Max value: -33dBm, RSU: -30dBm, Lab: -10dBm
    private static String pcapFilter = null;
    
    /**
     * Well-known Ethernet broadcast address
     */
    public static byte[] MAC_BROADCAST = new byte[]{(byte)0xFF,(byte)0xFF,(byte)0xFF,(byte)0xFF,(byte)0xFF,(byte)0xFF};
    
    /**
     * Parameter name for Link-Layer destination
     */
    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
     * @param  lowerStack   Lower protocol stack   
     */
    public CommsigniaLayer(IManagementLayers management, Stack<String> lowerStack) {
        super(management, lowerStack);
        
        MessageSet defaultMessageSet = MessageSet.C;
        itsApplication = new ITSApplication(ItsAid, TargetHost, TargetPort, defaultMessageSet);
        try {
            itsApplication.connect(1000);
            itsApplication.registerBlocking();
//            itsApplication.setFacilityModuleStatus(FacilityModule.BSM, false);
//            itsApplication.setFacilityModuleStatus(FacilityModule.CAM, false);
//            itsApplication. gnBindBlocking(BTPType.NONE, 65535);
        } catch (TimeoutException e) {
            e.printStackTrace();
        } catch (InterruptedException e) {
            e.printStackTrace();
        } catch (ClientException e) {
            e.printStackTrace();
        }
    }
    
    /* (non-Javadoc)
     * @see org.etsi.its.adapter.layers.Layer#register(org.etsi.its.adapter.layers.Layer)
     */
    @Override
    public void register(Layer upperLayer) {
        TERFactory.getInstance().logDebug(">>> CommsigniaLayer.register: " + upperLayer);
        
        if(registeredUpperLayer == null) {
            super.register(upperLayer);
            
            try {
                Method getEthernetType = registeredUpperLayer.getClass().getMethod("getEthernetType", (Class<?>[])null);
                if (getEthernetType != null) {
                    upperLayerFrameType = (Short) getEthernetType.invoke(registeredUpperLayer, (Object[]) null); 
                }
            } catch (SecurityException e) {
                e.printStackTrace();
            } catch (NoSuchMethodException e) {
                e.printStackTrace();
            } catch (IllegalArgumentException e) {
                e.printStackTrace();
            } catch (IllegalAccessException e) {
                e.printStackTrace();
            } catch (InvocationTargetException e) {
                e.printStackTrace();
            }
            
            localMacAddress = management.getLinkLayerAddress();
            PcapMultiplexer.getInstance().register(this, DeviceMacAddress, this.getEthernetType());
        }
        
        if(ClientsToMacs.isEmpty()) {
            pcapFilter = 
                "udp dst port " + SourcePort + 
//                " and not ether[0x56:4]==0x04e54800 and not ether[0x56:4]==0x04f02122 and not ether[0x56:4]==0xa60e3f16 and not ether[0x56:4]==0x04e54800 and not ether[0x56:4]==0x92232f44 and not ether[0x56:4]==0x4a74556d" + 
                 " and not ether[0x56:4]==0x4a74556d" + 
                 " and ";
        } else {
            TERFactory.getInstance().logDebug("CommsigniaLayer.register: Another Client !");
            pcapFilter = pcapFilter + " and ";
        }
        // Update Filter
        String strMacAddress = String.format("%02x%02x%02x%02x", localMacAddress[0], localMacAddress[1], localMacAddress[2], localMacAddress[3]);
//        String strMacAddress = String.format("%02x", localMacAddress[0]);
//        for(int i=1; i < localMacAddress.length; i++) {
//            strMacAddress += String.format(":%02x", localMacAddress[i]);
//        }
        //udp dst port 7493 && wlan src 8b:ad:f0:0d:01:02
        pcapFilter = pcapFilter + "not ether[0x56:4]==0x" + strMacAddress;
        // Reset filter
        System.out.println("Reset PCAP filter: " + pcapFilter);
        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));
        
        if (message.length <= 20+8+29+26) { // TODO To be refine
            // Skip it
            return;
        }
        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
            34 + // 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);
                }
            }
        }
        
    }
    
    /* (non-Javadoc)
     * @see org.etsi.its.adapter.layers.Layer#send(byte[], java.util.Map)
     */
    @Override
    public boolean send(byte[] message, Map<String, Object> params) {
        //TERFactory.getInstance().logDebug(">>> CommsigniaLayer.send: " + ByteHelper.byteArrayToString(message));
        
        byte[] dst = (byte[])params.get(LINK_LAYER_DESTINATION);
        if(dst == null) {
            dst = MAC_BROADCAST;
        }
            
        byte[] packet = ByteHelper.concat(
            dst,
            localMacAddress, 
            ByteHelper.intToByteArray(upperLayerFrameType, 2), 
            message
        );
        
        try {
            String dstAddress = String.format(
                "%02x:%02x:%02x:%02x:%02x:%02x", 
                dst[0], 
                dst[1], 
                dst[2], 
                dst[3], 
                dst[4], 
                dst[5]);
            String srcAddress = String.format(
                "%02x:%02x:%02x:%02x:%02x:%02x", 
                localMacAddress[0], 
                localMacAddress[1], 
                localMacAddress[2], 
                localMacAddress[3], 
                localMacAddress[4], 
                localMacAddress[5]);
            Builder build = new InjectData.Builder();
            build
                .withDstAddress(dstAddress)
                .withSrcAddress(srcAddress)
                .withType(Type.GNP)
                .withInterfaceID(InterfaceID)
                .withTxPowerDbm(TxPowerDbm) 
                .withData(message);
            InjectData injectData = build.build();
            //TERFactory.getInstance().logDebug("CommsigniaLayer.send: " + ByteHelper.byteArrayToString(injectData.getData()));
            
            itsApplication.sendOnRadioBlocking(injectData);
            //TERFactory.getInstance().logDebug("<<< CommsigniaLayer.send: " + ByteHelper.byteArrayToString(packet));
            return true;//super.send(packet, params);
        } catch (ClientException e) {
            e.printStackTrace();
        } catch (Exception ex) {
            ex.printStackTrace();
        }
        
        return false;
    }
    
    /* (non-Javadoc)
     * @see org.etsi.its.adapter.layers.Layer#unregister(org.etsi.its.adapter.layers.Layer)
     */
    @Override
    public void unregister(Layer upperLayer) {
        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;
                }
            }
        }
    }
    
    /* (non-Javadoc)
     * @see org.etsi.its.adapter.ports.IEthernetSpecific#getEthernetType()
     */
    @Override
    public short getEthernetType() {
        return (short)0x0800;
    }

    /**
     * Local Ethernet address 
     */
    private byte[] localMacAddress;
    
    /**
     * Upper layer's frame type
     */
    private short upperLayerFrameType;
    
    private ITSApplication itsApplication = null;
    
}