GnLayer.java

/**
 *  Implementation of ITS GeoNetworking layer (background thread)
 *  
 *  @author     ETSI / STF424
 *  @version    $URL$
 *              $Id$
 *
 */
package org.etsi.its.adapter.layers;

import java.util.Arrays;
import java.util.HashMap;
import java.util.Map;
import java.util.Stack;

import org.etsi.adapter.TERFactory;
import org.etsi.common.ByteHelper;
import org.etsi.its.adapter.IManagementLayers;
import org.etsi.ttcn.tci.CharstringValue;

/**
 *  Implementation of ITS GeoNetworking layer (background thread)
 */
public class GnLayer extends Layer implements Runnable, IEthernetSpecific {
	
	/**
	 * Implemented version of GeoNetworking specification
	 */
	private static final int GN_VERSION = 0;	

	/**
     * Parameter name for GeoNetworking packet type
     */
	public static final String GN_TYPE = "GnType";

	/**
     * Parameter name for GeoNetworking packet sub-type
     */
	public static final String GN_SUBTYPE = "GnSubType";

	/**
     * Parameter name for GeoNetworking payload type
     */
	public static final String GN_NEXTHEADER = "GnNextHeader";

    /**
     * Parameter name for destination position vector
     */
	public static final String GN_DEPV = "GnDePV";

    /**
     * Parameter name for Location Service's Target GN_Address 
     */
	public static final String GN_TARGETGNADDR = "GnTargetGnAddress";

    /**
     * Parameter name for destination area's latitude
     */
	public static final String GN_LATITUDE = "GnLatitude";

    /**
     * Parameter name for destination area's longitude
     */
	public static final String GN_LONGITUDE = "GnLongitude";

    /**
     * Parameter name for destination area's distance A
     */
	public static final String GN_DISTANCEA = "GnDistanceA";

    /**
     * Parameter name for destination area's distance B
     */
	public static final String GN_DISTANCEB = "GnDistanceB";

    /**
     * Parameter name for destination area's angle
     */
	public static final String GN_ANGLE = "GnAngle";
	

	/**
     * GeoNetworking header type for unknown messages
     */
	public static final int HT_ANY = 0;
	
	/**
     * GeoNetworking header type for beacon messages
     */
    public static final int HT_BEACON = 1;
	
    /**
     * GeoNetworking header type for GeoUnicast messages
     */
    public static final int HT_GEOUNICAST = 2;
	
    /**
     * GeoNetworking header type for GeoAnycast messages
     */
    public static final int HT_GEOANYCAST = 3;
	
    /**
     * GeoNetworking header type for GeoBroadcast messages
     */
    public static final int HT_GEOBROADCAST = 4;
	
    /**
     * GeoNetworking header type for Topology-scoped broadcast messages
     */
    public static final int HT_TSB = 5;
	
    /**
     * GeoNetworking header type for Location Service messages
     */
    public static final int HT_LS = 6;
	
    
    /**
     * Unspecified GeoNetworking header sub-type
     */    
	public static final int HST_UNSPECIFIED = 0;

    /**
     * Circle sub-type for GeoBroadcast/GeoAnycast messages
     */    
    public static final int HST_CIRCLE = 0;

    /**
     * Rectangle sub-type for GeoBroadcast/GeoAnycast messages
     */    
    public static final int HST_RECT = 1;

    /**
     * Ellipse sub-type for GeoBroadcast/GeoAnycast messages
     */    
    public static final int HST_ELIPSE = 2;

    /**
     * Single-hop sub-type for TSB messages
     */    
    public static final int HST_SINGLEHOP = 0;

    /**
     * Multi-hop sub-type for TSB messages
     */    
    public static final int HST_MULTIHOP = 1;

    /**
     * LS-Request sub-type for LS messages
     */    
    public static final int HST_LSREQUEST = 0;

    /**
     * LS-Reply sub-type for LS messages
     */    
    public static final int HST_LSREPLY = 1;

    private static final long ITS_REF_TIME = 1072915200000L;

	/**
     * Constructor
     * @param  management   Layer management instance
     * @param  lowerStack   Lower protocol stack   
     */
	public GnLayer(IManagementLayers management, Stack<String> lowerStack) {
		super(management, lowerStack);
		sequenceNumber = 0;
		running = true;
		beaconThread = new Thread(this);
		beaconThread.start();
	}
	
	/* (non-Javadoc)
     * @see org.etsi.its.adapter.ports.IEthernetSpecific#getEthernetType()
     */
    @Override
    public short getEthernetType() {
        
        // Retrieve EthernetType value
        Integer iutEthernetTypeValue = Integer.decode(((CharstringValue)TERFactory.getInstance().getTaParameter("IutEthernetTypeValue")).getString());
        return iutEthernetTypeValue.shortValue();
    }

	/**
     * Thread function for sending periodic beacons
     * @see java.lang.Runnable#run()
     */
	@Override
	public void run() {
		Map<String, Object> params = new HashMap<String, Object>();
		params.put(GN_TYPE, HT_BEACON);
		while(running) {
			if(management.getGnBeacon() != null) {
				send(null, params);
			}
			try {
				Thread.sleep(management.getGnBeaconInterval()); 
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
	}
	
	/* (non-Javadoc)
	 * @see org.etsi.its.adapter.layers.Layer#unregister(org.etsi.its.adapter.layers.Layer)
	 */
	@Override
	public void unregister(Layer upperLayer) {
		if(running) {
			running = false;
			try {
				beaconThread.join();
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
		super.unregister(upperLayer);
	}

	/* (non-Javadoc)
	 * @see org.etsi.its.adapter.layers.Layer#send(byte[], java.util.Map)
	 */
	@Override
	public boolean send(byte[] message, Map<String, Object> params) {
		
		byte [] extHdr = null;
		int ht;
		int hst;
		int hoplimit = 255;
		
		try {
			ht = (Integer)params.get(GN_TYPE);
		}
		catch (NullPointerException e) {
			ht = HT_ANY;
		}	
	
		try {
			hst = (Integer)params.get(GN_SUBTYPE);
		}
		catch (NullPointerException e) {
			hst = HST_UNSPECIFIED;
		}

		switch(ht) {
		case HT_LS:
			if(hst == HST_LSREPLY) {
				extHdr = createLsReplyHeader((byte[])params.get(GN_DEPV));
			}
			else {
				extHdr = createLsRequestHeader((byte[])params.get(GN_TARGETGNADDR));
			}
		break;	
		case HT_GEOUNICAST:
			extHdr = createGeoUnicastHeader((byte[])params.get(GN_DEPV));
		break;
		case HT_TSB:
			if(hst == HST_MULTIHOP) { 
				extHdr = createTsbHeader();
			}
			else {
				hoplimit = 1;
				extHdr = createShbHeader();
			}
		break;
		case HT_GEOBROADCAST:
		case HT_GEOANYCAST:
			extHdr = createGeoBroadcastHeader( 
					(Long)params.get(GN_LATITUDE),
					(Long)params.get(GN_LONGITUDE),
					(Integer)params.get(GN_DISTANCEA),
					(Integer)params.get(GN_DISTANCEB),
					(Integer)params.get(GN_ANGLE)
					);
		}
		
		byte[] basicHdr = createBasicHeader();
		byte[] commonHdr = createCommonHeader((String)params.get(GN_NEXTHEADER), ht, hst, (message == null)?0:message.length, hoplimit);
		
		return super.send(ByteHelper.concat(basicHdr, commonHdr, extHdr, message), params);
	}
	
	/* (non-Javadoc)
	 * @see org.etsi.its.adapter.layers.Layer#receive(byte[])
	 */
	@Override
	public void receive(byte[] message, Map<String, Object> lowerInfo) {
	
		byte[] basicHdr = new byte[4]; // TODO To be removed
		System.arraycopy(message, 4, basicHdr, 0, 4);
		
		byte[] versionNh = new byte[1];
		System.arraycopy(basicHdr, 0, versionNh, 0, 1);
		int nextHeader = (int)(versionNh[0] & (byte)0x0F);

		byte[] commonHdr = new byte[8];
		System.arraycopy(message, 4, commonHdr, 0, 8);
		
		byte[] htHst = new byte[1];
		System.arraycopy(commonHdr, 1, htHst, 0, 1);
		int headerSubType = (int)(htHst[0] & (byte)0x0F);
		int headerType = (int)(htHst[0] >> 4);
		
		byte[] pl = new byte[2];
		System.arraycopy(commonHdr, 4, pl , 0, 2);		
		int payloadLength = ByteHelper.byteArrayToInt(pl);
		
		if(headerType == HT_LS) {
			// Process LS messages
			if(headerSubType == HST_LSREQUEST) {
				byte[] gnAddress = new byte[8];
				System.arraycopy(message, 68, gnAddress, 0, 8);
				byte[] mid = new byte[6];
				System.arraycopy(gnAddress, 2, mid, 0, 6);
				if(Arrays.equals(mid, management.getLinkLayerAddress()) == true) {
					// Send LS Reply
					byte[] depv = new byte[20];
					System.arraycopy(message, 40, depv, 0, 20);
					
					Map<String, Object> params = new HashMap<String, Object>();
					params.put(GN_DEPV, depv);
					params.put(GN_TYPE, HT_LS);
					params.put(GN_SUBTYPE, HST_LSREPLY);
					send(null, params);
				}
			}
			else {
				// we are not interested in LS replies so far.
			}
		}		
		else {
			// Other messages
			if(payloadLength > 0) {
				byte[] payload = new byte[payloadLength];
				System.arraycopy(message, message.length - payloadLength, payload, 0, payloadLength);
				lowerInfo.put(GN_NEXTHEADER, nextHeader);
				super.receive(payload, lowerInfo);
			}
		}
	}

	/**
     * Builds encoded Basic Header
     * @return Encoded Basic Header
     */
    private byte[] createBasicHeader() {
        // Version 4 bits
        // NextHeader 4 bits
        byte[] versionNh = new byte[1];     
        int nh = 1;
        
        versionNh[0] = (byte)(GN_VERSION << 4); 
        versionNh[0] |= (byte)nh & 0x0F;
        
        // Reserved 1 byte
        byte[] reserved = new byte[]{(byte)0x00};
        
        // Lifetime 1 byte
        byte[] lifetime = new byte[]{(byte)0x2B};
        
        // RHL 1 byte
        byte[] rhl = new byte[]{(byte)0x0A};
        
        return ByteHelper.concat(versionNh, reserved, lifetime, rhl);
    }
	
	/**
	 * Builds encoded Common Header
	 * @param  nextHeader  Payload type 
	 * @param  ht          Packet type
	 * @param  hst         Packet sub-type
	 * @param  msgLength   Length of payload
	 * @return Encoded Common Header
	 */
	private byte[] createCommonHeader(String nextHeader, int ht, int hst, int msgLength, int hoplimit) {
		
		// Version 4 bits
		// NextHeader 4 bits
		byte[] nhReserved = new byte[1];		
		int nh = 0;
		if(nextHeader != null) {
			if(nextHeader.equals("BTP-A")) {
				nh = 1;
			}	
			else if(nextHeader.equals("BTP-B")) {
				nh = 2;
			}
			else if(nextHeader.equals("IPv6")) {
				nh = 3;
			}
		}
		nhReserved[0] = (byte)(nh << 4); 
		
		// HeaderType 4 bits
		// HeaderSubType 4 bits
		byte[] htHst = new byte[1];
		htHst[0] = (byte)(ht << 4); 
		htHst[0] |= (byte)hst & 0x0F; 
		
		// Traffic Class 1 byte
		byte[] tc = new byte[]{(byte)0x00};
		
		// Flags 1 byte
		byte[] flags = new byte[]{(byte)0x00};
		
		// PayloadLength 2 bytes
		byte[] pl = ByteHelper.intToByteArray(msgLength, 2);
		
		// Maximum HopLimit 1 byte
		byte[] mhl = ByteHelper.intToByteArray(hoplimit, 1); 
		
		// Reserved 1 byte
        byte[] reserved = new byte[]{(byte)0x00};
		
		return ByteHelper.concat(nhReserved, htHst, tc, flags, pl, mhl, reserved);		
	}
	
	/**
	 * Builds self GN_Address based on link-layer address
	 * @return Encoded GN_Address
	 */
	private byte[] createMyGnAddress() {		
		int ssc = 208; // France
		byte[] flags = new byte[2];
		flags[0] = (byte)(1 << 7); // Manual address
		flags[0] |= (byte)(9 << 3); // Ordinary RSU
		flags[0] |= (byte)(1 << 2); // Private
		flags[0] |= (byte)(ssc >> 10); // SSC
		flags[1] = (byte)ssc; // SSC
		byte[] mid = management.getLinkLayerAddress();
		return ByteHelper.concat(flags, mid);
	}
	
	/**
	 * Builds self long position vector
	 * @return Encoded long position vector
	 */
	private byte[] createMyLpv() { 
		
		byte[] gn = createMyGnAddress(); // 8 bytes 
		// Timestamp is 1s older than current time to avoid sending beacons coming from the future (time sync between nodes)
		byte[] tst = ByteHelper.intToByteArray((int)((System.currentTimeMillis() - ITS_REF_TIME) % (long)Math.pow(2,32) - 1000), 4);
		byte[] latitude = management.getLatitude();
		byte[] longitude = management.getLongitude();
		byte[] speed = ByteHelper.intToByteArray(0, 2);
		byte[] heading = ByteHelper.intToByteArray(0, 2);
		return ByteHelper.concat(gn, tst, latitude, longitude, speed, heading);
	}
	
	/**
	 * Builds GeoUnicast extension header
	 * @param  depv    Destination position vector (short position vector)
	 * @return Encoded GeoUnicast extension header 
	 */
	private byte[] createGeoUnicastHeader(byte[] depv) {			
		byte[] tsb = createTsbHeader();
		return ByteHelper.concat(tsb, depv);		
	}

	/**
     * Builds SHB extension header
     * @return Encoded SHB extension header
     */
    private byte[] createShbHeader() {
        byte[] sopv = createMyLpv();
        byte[] reserved = ByteHelper.intToByteArray(0, 4);
        return ByteHelper.concat(sopv, reserved);       
    }
	
	/**
	 * Builds TSB extension header
	 * @return Encoded TSB extension header
	 */
	private byte[] createTsbHeader() {
		byte[] sn = ByteHelper.intToByteArray(sequenceNumber++ , 2);
		byte[] reserved = ByteHelper.intToByteArray(0, 2);
		byte[] sopv = createMyLpv();
		return ByteHelper.concat(sn, reserved, sopv);		
	}
	
	/**
	 * Builds GeoBroadcast extension header
	 * @param geoAreaLatitude  Destination GeoArea's latitude
	 * @param geoAreaLongitude Destination GeoArea's longitude
	 * @param distanceA        Destination GeoArea's distance A
	 * @param distanceB        Destination GeoArea's distance B
	 * @param angle            Destination GeoArea's angle
	 * @return Encoded GeoBroadcast extension header
	 */
	private byte[] createGeoBroadcastHeader(long geoAreaLatitude, long geoAreaLongitude, int distanceA, int distanceB, int angle) {
		byte[] tsb = createTsbHeader();		
		byte[] lat = ByteHelper.longToByteArray(geoAreaLatitude & 0xffffffffL, 4);
		byte[] lng = ByteHelper.longToByteArray(geoAreaLongitude & 0xffffffffL, 4);
		byte[] distA = ByteHelper.intToByteArray(distanceA, 2);
		byte[] distB = ByteHelper.intToByteArray(distanceB, 2);
		byte[] ang = ByteHelper.intToByteArray(angle, 2);
		byte[] reserved = ByteHelper.intToByteArray(0, 2);
		return ByteHelper.concat(tsb, lat, lng, distA, distB, ang, reserved);		
	}
	
	/**
	 * Builds LS Request extension header
	 * @param  gnAddress    Target GN_Address
	 * @return Encoded LS Request extension header
	 */
	private byte[] createLsRequestHeader(byte[] gnAddress) {
		byte[] tsb = createTsbHeader();
		return ByteHelper.concat(tsb, gnAddress);		
	}
	
	/**
	 * Builds LS Reply extension header 
	 * @param  depv    Destination position vector (short position vector)
	 * @return Encoded LS Reply extension header
	 */
	private byte[] createLsReplyHeader(byte[] depv) {			
		byte[] tsb = createTsbHeader();
		return ByteHelper.concat(tsb, depv);	
	}

	/**
     * Indicates whether the layer is still active. Setting this field to false will cause
     * the beaconning thread to stop its execution.
     */
	private boolean running;
	
	/**
     * Beaconning thread instance.
     */
	private Thread beaconThread;
	
	/**
	 * Packet sequence number. Incremented after sending each unicast packet 
	 */
	private int sequenceNumber;
}