STF507 Week#11: Merge GNSS support from C2C branch (f5926bea) · Commits · ITS - Intelligent Transport Systems / ITS

javasrc/adapter/org/etsi/adapter/IGnssSupport.java

+30 −0

Original line number	Original line	Diff line number	Diff line
	@@ -31,6 +31,9 @@
	*/		*/
	package org.etsi.adapter;		package org.etsi.adapter;

			import java.math.BigInteger;

			import org.etsi.its.adapter.ports.AdapterPort;
	import org.etsi.ttcn.tri.TriStatus;		import org.etsi.ttcn.tri.TriStatus;

	/**		/**
	@@ -63,6 +66,33 @@ public interface IGnssSupport {
	*/		*/
	public boolean stopScenario();		public boolean stopScenario();

			/**
			* Signals when the requested distance was covered
			* @param distance The requested distance in m.
			* @return TRUE on success, FALSE otherwise
			*/
			public boolean awaitDistanceToCover(AdapterPort notifier, double distance);

			/**
			* Changes dynamically the speed in the running scenario
			* @param The new speed value.
			* @return TRUE on success, FALSE otherwise
			*/
			public boolean changeSpeed(double speed);

			/**
			* Changes dynamically the heading in the running scenario
			* @param heading The new heading value.
			* @return TRUE on success, FALSE otherwise
			*/
			public boolean changeHeading(double heading);

			/**
			* Gets the current GPS time
			* @return the time
			*/
			public BigInteger getGpsTime();

	/**		/**
	* Dispose object		* Dispose object
	*/		*/

javasrc/adapter/org/etsi/its/adapter/SecurityHelper.java

+51 −14

Original line number	Original line	Diff line number	Diff line
	@@ -112,11 +112,18 @@ public class SecurityHelper {
	return null;		return null;
	}		}
	}		}
	byte[] aaSigningPublicKeyX, aaSigningPublicKeyY;		byte[] aaSigningPublicKeyX = null, aaSigningPublicKeyY = null;
	aaSigningPublicKeyX = ByteHelper.extract(certificateKeys.toByteArray(), 0, 32);		byte[] keys = certificateKeys.toByteArray();
			if ((keys[0] == 0x02) \|\| (keys[0] == 0x03)) { // Key length = 32 bytes
			aaSigningPublicKeyX = ByteHelper.extract(keys, 1, 32);
	System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: aaSigningPublicKeyX:" + ByteHelper.byteArrayToString(aaSigningPublicKeyX));		System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: aaSigningPublicKeyX:" + ByteHelper.byteArrayToString(aaSigningPublicKeyX));
	aaSigningPublicKeyY = ByteHelper.extract(certificateKeys.toByteArray(), 32, 32);		} else { // Key length = 64 bytes
			aaSigningPublicKeyX = ByteHelper.extract(keys, 1, 32);
	System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: aaSigningPublicKeyX:" + ByteHelper.byteArrayToString(aaSigningPublicKeyX));		System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: aaSigningPublicKeyX:" + ByteHelper.byteArrayToString(aaSigningPublicKeyX));
			aaSigningPublicKeyY = ByteHelper.extract(keys, 33, 32);
			System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: aaSigningPublicKeyX:" + ByteHelper.byteArrayToString(aaSigningPublicKeyX));
			}
			// FIXME Add encryption support
	System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: headerFields:" + ByteHelper.byteArrayToString(headerFields));		System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: headerFields:" + ByteHelper.byteArrayToString(headerFields));
	// Extract payload, decvalue is updated with the payload		// Extract payload, decvalue is updated with the payload
	if (decvalue.read() != 1) {		if (decvalue.read() != 1) {
	@@ -149,12 +156,14 @@ public class SecurityHelper {
	p_message.length - (int)(p_offset + secureTrailerLength - 1 /* Exclude signature structure but keep signature type and signature length */)		p_message.length - (int)(p_offset + secureTrailerLength - 1 /* Exclude signature structure but keep signature type and signature length */)
	);		);
	System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload:" + ByteHelper.byteArrayToString(toBeVerifiedData));		System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload:" + ByteHelper.byteArrayToString(toBeVerifiedData));
	// Calculate Digest digest from the buffer toBeVerifiedData
	byte[] hash = CryptoLib.hashWithSha256(toBeVerifiedData);
	boolean result;		boolean result;
	try {		try {
			if (aaSigningPublicKeyY == null) {
			// FIXME FSCOM: Check how t verify compressed signature
			return payload;
			}
	result = CryptoLib.verifyWithEcdsaNistp256WithSha256(		result = CryptoLib.verifyWithEcdsaNistp256WithSha256(
	hash,		toBeVerifiedData,
	signature.toByteArray(),		signature.toByteArray(),
	aaSigningPublicKeyX,		aaSigningPublicKeyX,
	aaSigningPublicKeyY		aaSigningPublicKeyY
	@@ -163,7 +172,8 @@ public class SecurityHelper {
	if (!result) {		if (!result) {
	// Drop packet		// Drop packet
	System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: toBeVerifiedData :" + ByteHelper.byteArrayToString(toBeVerifiedData));		System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: toBeVerifiedData :" + ByteHelper.byteArrayToString(toBeVerifiedData));
	System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: Hash :" + ByteHelper.byteArrayToString(hash));		// Calculate Digest digest from the buffer toBeVerifiedData
			System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: Hash :" + ByteHelper.byteArrayToString(CryptoLib.hashWithSha256(toBeVerifiedData)));
	System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: signature :" + ByteHelper.byteArrayToString(signature.toByteArray()));		System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: signature :" + ByteHelper.byteArrayToString(signature.toByteArray()));
	System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: aaSigningPublicKeyX:" + ByteHelper.byteArrayToString(aaSigningPublicKeyX));		System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: aaSigningPublicKeyX:" + ByteHelper.byteArrayToString(aaSigningPublicKeyX));
	System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: aaSigningPublicKeyY:" + ByteHelper.byteArrayToString(aaSigningPublicKeyY));		System.out.println("SecurityHelper.checkSecuredProfileAndExtractPayload: aaSigningPublicKeyY:" + ByteHelper.byteArrayToString(aaSigningPublicKeyY));
	@@ -404,11 +414,24 @@ public class SecurityHelper {
	ByteArrayInputStream subjectAttributes = new ByteArrayInputStream(b);		ByteArrayInputStream subjectAttributes = new ByteArrayInputStream(b);
	if (subjectAttributes.read() == 0x00) { // Subject Attribute: verification key (0) - Mandatory		if (subjectAttributes.read() == 0x00) { // Subject Attribute: verification key (0) - Mandatory
	if (subjectAttributes.read() == 0x00) { // Public Key Alg: ecdsa nistp256 with sha256 (0)		if (subjectAttributes.read() == 0x00) { // Public Key Alg: ecdsa nistp256 with sha256 (0)
	if (subjectAttributes.read() == 0x04) { // ECC Point Type: uncompressed (4)		byte v = (byte) subjectAttributes.read();
			p_keys.write(v);
			System.out.println("SecurityHelper.decodeCertificate: ECC Point Type: =" + v);
			if (v == 0x02) { // ECC Point Type: compressed lsb y-0(2)
	byte[] key = new byte[32];		byte[] key = new byte[32];
	subjectAttributes.read(key, 0, 32);		subjectAttributes.read(key, 0, 32);
			System.out.println("SecurityHelper.decodeCertificate: Verification lsb y-1 key=" + ByteHelper.byteArrayToString(key));
	p_keys.write(key);		p_keys.write(key);
			} else if (v == 0x03) { // ECC Point Type: compressed lsb y-1(3)
			byte[] key = new byte[32];
			subjectAttributes.read(key, 0, 32);
			System.out.println("SecurityHelper.decodeCertificate: Verification lsb y-1 key=" + ByteHelper.byteArrayToString(key));
			p_keys.write(key);
			} else if (v == 0x04) { // ECC Point Type: uncompressed (4)
			byte[] key = new byte[32];
			subjectAttributes.read(key, 0, 32);
	System.out.println("SecurityHelper.decodeCertificate: Verification key1=" + ByteHelper.byteArrayToString(key));		System.out.println("SecurityHelper.decodeCertificate: Verification key1=" + ByteHelper.byteArrayToString(key));
			p_keys.write(key);
	subjectAttributes.read(key, 0, 32);		subjectAttributes.read(key, 0, 32);
	System.out.println("SecurityHelper.decodeCertificate: Verification key2=" + ByteHelper.byteArrayToString(key));		System.out.println("SecurityHelper.decodeCertificate: Verification key2=" + ByteHelper.byteArrayToString(key));
	p_keys.write(key);		p_keys.write(key);
	@@ -421,14 +444,27 @@ public class SecurityHelper {
	if (v == 0x01) { // // Subject Attribute: encryption key (1)		if (v == 0x01) { // // Subject Attribute: encryption key (1)
	if (subjectAttributes.read() == 0x01) { // Public Key Alg: ecdsa nistp256 (1)		if (subjectAttributes.read() == 0x01) { // Public Key Alg: ecdsa nistp256 (1)
	if (subjectAttributes.read() == 0x00) { // Symmetric Algorithm: aes 128 ccm (0)		if (subjectAttributes.read() == 0x00) { // Symmetric Algorithm: aes 128 ccm (0)
	if (subjectAttributes.read() == 0x04) { // ECC Point Type: uncompressed (4)		v = (byte) subjectAttributes.read();
			p_keys.write(v);
			System.out.println("SecurityHelper.decodeCertificate: ECC Point Type: =" + v);
			if (v == 0x02) { // ECC Point Type: compressed lsb y-0(2)
			byte[] key = new byte[32];
			subjectAttributes.read(key, 0, 32);
			System.out.println("SecurityHelper.decodeCertificate: Encryption lsb y-0 key=" + ByteHelper.byteArrayToString(key));
			p_keys.write(key);
			} else if (v == 0x03) { // ECC Point Type: compressed lsb y-1(3)
			byte[] key = new byte[32];
			subjectAttributes.read(key, 0, 32);
			System.out.println("SecurityHelper.decodeCertificate: Encryption lsb y-1 key=" + ByteHelper.byteArrayToString(key));
			p_keys.write(key);
			} else if (v == 0x04) { // ECC Point Type: uncompressed (4)
	byte[] key = new byte[32];		byte[] key = new byte[32];
	subjectAttributes.read(key, 0, 32);		subjectAttributes.read(key, 0, 32);
	System.out.println("SecurityHelper.decodeCertificate: Encryption key1=" + ByteHelper.byteArrayToString(key));		System.out.println("SecurityHelper.decodeCertificate: Encryption key1=" + ByteHelper.byteArrayToString(key));
	//p_keys.write(key);		p_keys.write(key);
	subjectAttributes.read(key, 0, 32);		subjectAttributes.read(key, 0, 32);
	System.out.println("SecurityHelper.decodeCertificate: Encryption key2=" + ByteHelper.byteArrayToString(key));		System.out.println("SecurityHelper.decodeCertificate: Encryption key2=" + ByteHelper.byteArrayToString(key));
	//p_keys.write(key);		p_keys.write(key);
	} // FIXME To be continued		} // FIXME To be continued
	} // FIXME To be continued		} // FIXME To be continued
	} // FIXME To be continued		} // FIXME To be continued
	@@ -670,7 +706,8 @@ public class SecurityHelper {
	int secureTrailerIndex = 0;		int secureTrailerIndex = 0;
	if (p_secureTrailer[secureTrailerIndex++] == 0x01) { // Trailer Type: signature (1)		if (p_secureTrailer[secureTrailerIndex++] == 0x01) { // Trailer Type: signature (1)
	if (p_secureTrailer[secureTrailerIndex++] == 0x00) { // Public Key Alg: ecdsa nistp256 with sha256 (0)		if (p_secureTrailer[secureTrailerIndex++] == 0x00) { // Public Key Alg: ecdsa nistp256 with sha256 (0)
	if (p_secureTrailer[secureTrailerIndex++] == 0x02) { // ECC Point Type: compressed lsb y-0 (2)		byte v = p_secureTrailer[secureTrailerIndex++];
			if ((v == 0x00) \|\| (v == 0x02)) { // ECC Point Type: compressed lsb y-0 (2)
	if (p_secureTrailer.length == (3 + 2 * 32)) {		if (p_secureTrailer.length == (3 + 2 * 32)) {
	// Build the signature vector		// Build the signature vector
	try {		try {

javasrc/adapter/org/etsi/its/adapter/layers/GnLayer.java

+1 −1

Original line number	Original line	Diff line number	Diff line
	@@ -418,7 +418,7 @@ public class GnLayer extends Layer implements Runnable, IEthernetSpecific {
	// Other messages		// Other messages
	if(payloadLength > 0) {		if(payloadLength > 0) {
	byte[] mpayload = new byte[payloadLength];		byte[] mpayload = new byte[payloadLength];
	System.arraycopy(payload, commonHdr.length + 28/Topology-Scoped Broadcast/, mpayload, 0, payloadLength);		System.arraycopy(payload, commonHdr.length + 44/Topology-Scoped Broadcast/, mpayload, 0, payloadLength);
	// System.out.println("GnLayer.receive: Message =" + ByteHelper.byteArrayToString(mpayload));		// System.out.println("GnLayer.receive: Message =" + ByteHelper.byteArrayToString(mpayload));
	lowerInfo.put(GN_NEXTHEADER, nextHeader);		lowerInfo.put(GN_NEXTHEADER, nextHeader);
	lowerInfo.put(GN_TYPE, headerType);		lowerInfo.put(GN_TYPE, headerType);

javasrc/adapter/org/etsi/its/adapter/ports/AdapterControlPort.java

+31 −4

Original line number	Original line	Diff line number	Diff line
	@@ -30,7 +30,8 @@ public class AdapterControlPort extends AdapterPort implements IPort, IObservabl
	/* AdapterControl Response */		/* AdapterControl Response */
	private static final byte AcGnResponse = 0;		private static final byte AcGnResponse = 0;
	//private static final byte AcGn6Response = 1;		//private static final byte AcGn6Response = 1;
	private static final byte AcGnssResponse = 2;		public static final byte AcGnssResponse = 2;
			public static final byte AcGnssDistanceCovered = 3;

	/* GN Commands */		/* GN Commands */
	private static final byte AcStartBeaconing = 0;		private static final byte AcStartBeaconing = 0;
	@@ -51,9 +52,12 @@ public class AdapterControlPort extends AdapterPort implements IPort, IObservabl
	private static final byte AcLoadScenario = 0x70;		private static final byte AcLoadScenario = 0x70;
	private static final byte AcStartScenario = 0x71;		private static final byte AcStartScenario = 0x71;
	private static final byte AcStopScenario = 0x72;		private static final byte AcStopScenario = 0x72;
			private static final byte AcAwaitDistanceToCover = 0x73;
			private static final byte AcChangeSpead = 0x74;
			private static final byte AcChangeHeading = 0x75;

	protected static final byte AcTrue = 0x01;		public static final byte AcTrue = 0x01;
	protected static final byte AcFalse = 0x00;		public static final byte AcFalse = 0x00;

	private static final String GNSS_SCENARIO_SUPPORT = "GnssScenarioSupport";		private static final String GNSS_SCENARIO_SUPPORT = "GnssScenarioSupport";
	private IGnssSupport GNSS;		private IGnssSupport GNSS;
	@@ -150,7 +154,7 @@ public class AdapterControlPort extends AdapterPort implements IPort, IObservabl
	byte[] data = ByteHelper.extract(message, 2, message.length - 2);		byte[] data = ByteHelper.extract(message, 2, message.length - 2);
	switch (message[1]) {		switch (message[1]) {
	case AcLoadScenario:		case AcLoadScenario:
	LoadScenario(data[0]);		LoadScenario(ByteHelper.byteArrayToShort(data));
	break;		break;
	case AcStartScenario:		case AcStartScenario:
	StartScenario();		StartScenario();
	@@ -158,6 +162,16 @@ public class AdapterControlPort extends AdapterPort implements IPort, IObservabl
	case AcStopScenario:		case AcStopScenario:
	StopScenario();		StopScenario();
	break;		break;
			case AcAwaitDistanceToCover:
			Float distance = ByteHelper.byteArrayToFloat(data);
			AwaitDistanceToCover(distance);
			break;
			case AcChangeSpead:
			// ChangeSpeed(speed);
			break;
			case AcChangeHeading:
			// ChangeHeading(heading);
			break;
	}		}
	}		}
	else {		else {
	@@ -256,6 +270,19 @@ public class AdapterControlPort extends AdapterPort implements IPort, IObservabl
	}).start();		}).start();
	}		}

			private void AwaitDistanceToCover(final double distance) {
			new Thread(new Runnable() {
			@Override
			public void run() {
			boolean result = GNSS.awaitDistanceToCover(AdapterControlPort.this, distance);
			byte[] response = {(byte)AcGnssResponse, (byte)(result?AcFalse:AcTrue)};

			setChanged();
			notifyObservers(new PortEvent(response, getPortName(), getComponentName()));
			}
			}).start();
			}

	@Override		@Override
	public void dispose() {		public void dispose() {
	if (gnssScenarioSupport) {		if (gnssScenarioSupport) {

javasrc/adapter/org/etsi/its/adapter/ports/AdapterPort.java

+1 −1

Original line number	Original line	Diff line number	Diff line
	@@ -45,7 +45,7 @@ public abstract class AdapterPort implements IPort, IObservable {
	/**		/**
	* Marks this Observable object as having been changed		* Marks this Observable object as having been changed
	*/		*/
	protected void setChanged() {		public void setChanged() {
	observable.setChanged();		observable.setChanged();
	}		}