From 4e80b97050c066f52d6560daaaf6295863930737 Mon Sep 17 00:00:00 2001
From: Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
Date: Mon, 12 Feb 2018 16:36:11 -0800
Subject: [PATCH] Galois Embedded Crypto module updates (#2235)

* Bump rustlink

* Changes in GEC, now communication and key exchange works. TODO is key renegotiation and some cleanup

* Bump pprzlink

* Update in GEC and related control panel configs

* Updated name

* Bump rustlink

* Changes in GEC, now communication and key exchange works. TODO is key renegotiation and some cleanup

* Bump pprzlink

* Update in GEC and related control panel configs

* Update atomic settings

* Minor changes, style fix

* Code style fix

* More code style fix
---
 .../AGGIEAIR/aggieair_atomic_lia.xml          |   7 +-
 conf/airframes/AGGIEAIR/aggieair_conf.xml     |   1 -
 .../AGGIEAIR/aggieair_control_panel.xml       |  42 +-
 conf/modules/telemetry_transparent_secure.xml |  49 --
 sw/airborne/modules/datalink/gec/gec.c        |  97 +--
 sw/airborne/modules/datalink/gec/gec.h        | 100 ++-
 sw/airborne/modules/datalink/gec_dl.c         | 769 +++++++++++++-----
 sw/airborne/modules/datalink/gec_dl.h         |  43 +-
 sw/airborne/subsystems/datalink/datalink.h    |   1 +
 sw/ext/rustlink                               |   2 +-
 10 files changed, 742 insertions(+), 369 deletions(-)
 delete mode 100644 conf/modules/telemetry_transparent_secure.xml

diff --git a/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml b/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml
index abea78e9e7..691303ff38 100644
--- a/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml
@@ -11,8 +11,8 @@ AggieAir Atomic Tangerine
       <module name="radio_control" type="sbus">
         <configure name="SBUS_PORT" value="UART5"/>
       </module>
-      <configure name="HAS_LUFTBOOT" value="0"/>
-      <configure name="FLASH_MODE" value="SWD"/>
+      <configure name="HAS_LUFTBOOT" value="1"/>
+      <configure name="FLASH_MODE" value="DFU"/>
     </target>
 
     <!-- NOTE: if you want to use extra_dl module for HITL
@@ -39,9 +39,10 @@ AggieAir Atomic Tangerine
     <module name="control"/>
     <module name="navigation"/>
 
-    <module name="telemetry"   type="transparent">
+    <module name="telemetry"   type="transparent_gec">
       <configure name="MODEM_PORT"        value="UART3"/>
       <configure name="MODEM_BAUD"        value="B57600"/>
+      <define name="GEC_STATUS_LED" value="5"/>
     </module>
 
     <module name="ins"       type="vectornav">
diff --git a/conf/airframes/AGGIEAIR/aggieair_conf.xml b/conf/airframes/AGGIEAIR/aggieair_conf.xml
index 99c751e739..4c34877a55 100644
--- a/conf/airframes/AGGIEAIR/aggieair_conf.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_conf.xml
@@ -31,7 +31,6 @@
    settings="settings/fixedwing_basic.xml settings/nps.xml"
    settings_modules="modules/battery_monitor.xml modules/lidar_sf11.xml modules/nav_skid_landing.xml modules/nav_survey_poly_osam.xml modules/gps.xml modules/nav_basic_fw.xml modules/guidance_basic_fw.xml modules/stabilization_attitude_fw.xml"
    gui_color="#ffff42870000"
-   release=""
   />
   <aircraft
    name="BluJay_Goose"
diff --git a/conf/airframes/AGGIEAIR/aggieair_control_panel.xml b/conf/airframes/AGGIEAIR/aggieair_control_panel.xml
index 1e52986d40..e1b381b630 100644
--- a/conf/airframes/AGGIEAIR/aggieair_control_panel.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_control_panel.xml
@@ -20,7 +20,7 @@
     <program name="Secure Link" command="sw/ext/rustlink/link">
       <arg flag="-d" constant="/dev/ttyUSB0"/>
       <arg flag="-a" constant="@AC_ID"/>
-      <arg flag="-n" constant="@ACIRCRAFT"/>
+      <arg flag="-n" constant="@AIRCRAFT"/>
       <arg flag="-c" />
     </program>
     <program name="Link Combiner" command="sw/ground_segment/python/redundant_link/link_combiner.py"/>
@@ -107,6 +107,46 @@
      <program name="Real-time Plotter"/>
    </session>
 
+   <session name="Secure Flight USB-serial@57600">
+     <program name="Secure Link">
+       <arg flag="-d" constant="/dev/ttyUSB0"/>
+       <arg flag="-s" constant="57600"/>
+       <arg flag="-a" constant="@AC_ID"/>
+       <arg flag="-n" constant="@AIRCRAFT"/>
+       <arg flag="-c" />
+     </program>
+     <program name="Server"/>
+     <program name="GCS">
+      <arg flag="-layout" constant="AGGIEAIR/vertical.xml"/>
+     </program>
+     <program name="Messages"/>
+     <program name="Real-time Plotter"/>
+   </session>
+
+   <session name="Secure Simulation">
+     <program name="Secure Link">
+       <arg flag="--udp"/>
+       <arg flag="-a" constant="@AC_ID"/>
+       <arg flag="-n" constant="@AIRCRAFT"/>
+       <arg flag="-c" />
+     </program>
+     <program name="Server">
+      <arg flag="-n"/>
+     </program>
+     <program name="GCS">
+      <arg flag="-layout" constant="AGGIEAIR/vertical.xml"/>
+     </program>
+     <program name="Simulator">
+      <arg flag="-a" constant="@AIRCRAFT"/>
+      <arg flag="-f" constant="127.0.0.1"/>
+      <arg flag="-b" constant="127.255.255.255"/>
+      <arg flag="--fg_fdm"/>
+      <arg flag="--nodisplay"/>
+     </program>
+     <program name="Messages"/>
+     <program name="Environment Simulator"/>
+   </session>
+
    <session name="Flight USB-serial@115200">
      <program name="Data Link">
        <arg flag="-d" constant="/dev/ttyUSB0"/>
diff --git a/conf/modules/telemetry_transparent_secure.xml b/conf/modules/telemetry_transparent_secure.xml
deleted file mode 100644
index bd77d1e1a8..0000000000
--- a/conf/modules/telemetry_transparent_secure.xml
+++ /dev/null
@@ -1,49 +0,0 @@
-<!DOCTYPE module SYSTEM "module.dtd">
-
-<module name="telemetry_transparent_secure" dir="datalink" task="datalink">
-  <doc>
-    <description>
-      Telemetry using secure PPRZ protocol over UART
-      Similar to telemetry_transparent, except it uses message scheduling and encryption
-    </description>
-    <configure name="MODEM_PORT" value="UARTx" description="UART where the modem is connected to (UART1, UART2, etc)"/>
-    <configure name="MODEM_BAUD" value="B57600" description="UART baud rate"/>
-  </doc>
-  <autoload name="telemetry" type="nps"/>
-  <autoload name="telemetry" type="sim"/>
-  <autoload name="haclc"/>
-  <header>
-    <file name="spprz_dl.h"/>
-  </header>
-  <init fun="spprz_dl_init()"/>
-  <periodic fun="spprz_dl_periodic()" freq="PERIODIC_FREQUENCY" autorun="TRUE"/>
-  <event fun="spprz_dl_event()"/>
-  <makefile target="!fbw|sim|nps|hitl">
-    <configure name="MODEM_PORT" case="upper|lower"/>
-    <define name="USE_$(MODEM_PORT_UPPER)"/>
-    <define name="$(MODEM_PORT_UPPER)_BAUD" value="$(MODEM_BAUD)"/>
-    <define name="DOWNLINK"/>
-    <define name="PERIODIC_TELEMETRY"/>
-    <define name="DOWNLINK_DEVICE" value="$(MODEM_PORT_LOWER)"/>
-    <define name="PPRZ_UART" value="$(MODEM_PORT_LOWER)"/>
-    <define name="DOWNLINK_TRANSPORT" value="spprz_tp"/>
-    <define name="DATALINK" value="PPRZ"/>
-    <file name="spprz_dl.c"/>
-    <file name="downlink.c" dir="subsystems/datalink"/>
-    <file name="datalink.c" dir="subsystems/datalink"/>
-    <file name="telemetry.c" dir="subsystems/datalink"/>
-    <file name="secure_pprz_transport.c" dir="pprzlink/src"/>
-  </makefile>
-  <makefile target="ap" firmware="fixedwing">
-    <file name="fixedwing_datalink.c" dir="$(SRC_FIRMWARE)"/>
-    <file name="ap_downlink.c" dir="$(SRC_FIRMWARE)"/>
-  </makefile>
-  <makefile target="ap|fbw" firmware="fixedwing">
-    <file name="fbw_downlink.c" dir="$(SRC_FIRMWARE)"/>
-  </makefile>
-  <makefile target="ap" firmware="rotorcraft">
-    <file name="rotorcraft_datalink.c" dir="$(SRC_FIRMWARE)"/>
-    <file name="rotorcraft_telemetry.c" dir="$(SRC_FIRMWARE)"/>
-  </makefile>
-</module>
-
diff --git a/sw/airborne/modules/datalink/gec/gec.c b/sw/airborne/modules/datalink/gec/gec.c
index 894a840d40..a10d822a21 100644
--- a/sw/airborne/modules/datalink/gec/gec.c
+++ b/sw/airborne/modules/datalink/gec/gec.c
@@ -26,27 +26,38 @@
  */
 #include "modules/datalink/gec/gec.h"
 
-void gec_sts_init(struct gec_sts_ctx * sts)
+void gec_sts_init(struct gec_sts_ctx *sts)
 {
-  clear_sts(sts);
+  // reset all keys
+  gec_clear_sts(sts);
 
+  // load their public key
   uint8_t theirPublicKey[PPRZ_KEY_LEN] = GCS_PUBLIC;
-  memcpy(&sts->theirPublicKey, theirPublicKey, PPRZ_KEY_LEN);
+  memcpy(&sts->their_public_key, theirPublicKey, PPRZ_KEY_LEN);
+  sts->their_public_key.ready = true;
 
+  // load my private key
   uint8_t myPublicKey[PPRZ_KEY_LEN] = UAV_PUBLIC;
-  memcpy(&sts->myPrivateKey.pub, myPublicKey, PPRZ_KEY_LEN);
-
+  memcpy(&sts->my_private_key.pub, myPublicKey, PPRZ_KEY_LEN);
   uint8_t myPrivateKey[PPRZ_KEY_LEN] = UAV_PRIVATE;
-  memcpy(&sts->myPrivateKey.priv, myPrivateKey, PPRZ_KEY_LEN);
+  memcpy(&sts->my_private_key.priv, myPrivateKey, PPRZ_KEY_LEN);
+  sts->my_private_key.ready = true;
 
+  // generate ephemeral key
+  gec_generate_ephemeral_keys(&sts->my_private_ephemeral);
 }
 
-void clear_sts(struct gec_sts_ctx * sts)
+void gec_clear_sts(struct gec_sts_ctx *sts)
 {
-  memset(&sts->theirPublicKeyEphemeral, 0, sizeof(struct gec_pubkey));
-  memset(&sts->myPrivateKeyEphemeral, 0, sizeof(struct gec_privkey));
-  memset(&sts->theirSymmetricKey, 0, sizeof(struct gec_sym_key));
-  memset(&sts->mySymmetricKey, 0, sizeof(struct gec_sym_key));
+  memset(&sts->their_public_ephemeral, 0, sizeof(struct gec_pubkey));
+  sts->their_public_ephemeral.ready = false;
+  memset(&sts->my_private_ephemeral, 0, sizeof(struct gec_privkey));
+  sts->my_private_ephemeral.ready = false;
+  memset(&sts->rx_sym_key, 0, sizeof(struct gec_sym_key));
+  sts->rx_sym_key.ready = false;
+  memset(&sts->tx_sym_key, 0, sizeof(struct gec_sym_key));
+  sts->tx_sym_key.ready = false;
+
   sts->protocol_stage = WAIT_MSG1;
   sts->party = RESPONDER;
   sts->last_error = ERROR_NONE;
@@ -55,73 +66,45 @@ void clear_sts(struct gec_sts_ctx * sts)
 /**
  * Generate private and public key pairs for future use.
  */
-void generate_ephemeral_keys(struct gec_privkey *sk)
+void gec_generate_ephemeral_keys(struct gec_privkey *sk)
 {
-  for (uint16_t i = 0; i < (PPRZ_KEY_LEN / sizeof(uint32_t)); i++) {
+  for (uint16_t i = 0; i < PPRZ_KEY_LEN; i += sizeof(uint32_t)) {
     uint32_t tmp = rng_wait_and_get();
     sk->priv[i] = (uint8_t) tmp;
-    sk->priv[i + 1] = (uint8_t) (tmp >> 8);
-    sk->priv[i + 2] = (uint8_t) (tmp >> 16);
-    sk->priv[i + 3] = (uint8_t) (tmp >> 24);
+    sk->priv[i + 1] = (uint8_t)(tmp >> 8);
+    sk->priv[i + 2] = (uint8_t)(tmp >> 16);
+    sk->priv[i + 3] = (uint8_t)(tmp >> 24);
   }
   uint8_t basepoint[32] = {0};
   basepoint[0] = 9; // default basepoint
   Hacl_Curve25519_crypto_scalarmult(sk->pub, sk->priv, basepoint);
+  sk->ready = true;
 }
 
 
 /**
  * Derive key material for both sender and receiver
  */
-void derive_key_material(struct gec_sts_ctx *ctx, uint8_t* z) {
-  uint8_t tmp[PPRZ_KEY_LEN*2] = {0};
-  uint8_t input[PPRZ_KEY_LEN+1] = {0};
+void gec_derive_key_material(struct gec_sts_ctx *ctx, uint8_t *z)
+{
+  uint8_t tmp[PPRZ_KEY_LEN * 2] = {0};
+  uint8_t input[PPRZ_KEY_LEN + 1] = {0};
 
   // Ka|| Sa = kdf(z,0)
   memcpy(input, z, PPRZ_KEY_LEN);
   input[PPRZ_KEY_LEN] = 0;
   Hacl_SHA2_512_hash(tmp, input, sizeof(input));
-  memcpy(ctx->theirSymmetricKey.key, tmp, PPRZ_KEY_LEN); // K_a
-  memcpy(ctx->theirSymmetricKey.nonce, &tmp[PPRZ_KEY_LEN], PPRZ_NONCE_LEN); // S_a
+  memcpy(ctx->rx_sym_key.key, tmp, PPRZ_KEY_LEN); // K_a
+  memcpy(ctx->rx_sym_key.nonce, &tmp[PPRZ_KEY_LEN], PPRZ_NONCE_LEN); // S_a
+  ctx->rx_sym_key.counter = 0;
+  ctx->rx_sym_key.ready = true;
 
   // Kb|| Sb = kdf(z,1)
   input[PPRZ_KEY_LEN] = 1;
   Hacl_SHA2_512_hash(tmp, input, sizeof(input));
-  memcpy(ctx->mySymmetricKey.key, tmp, PPRZ_KEY_LEN);  // K_b
-  memcpy(ctx->mySymmetricKey.nonce, &tmp[PPRZ_KEY_LEN], PPRZ_NONCE_LEN);  // S_b
+  memcpy(ctx->tx_sym_key.key, tmp, PPRZ_KEY_LEN);  // K_b
+  memcpy(ctx->tx_sym_key.nonce, &tmp[PPRZ_KEY_LEN], PPRZ_NONCE_LEN);  // S_b
+  ctx->tx_sym_key.counter = 0;
+  ctx->tx_sym_key.ready = true;
 }
 
-
-/**
- * Decrypt a message, no AAD for now
- * if res == 0 everything OK
- */
-uint32_t gec_decrypt(struct gec_sym_key *k, uint8_t *plaintext, uint8_t *ciphertext, uint8_t len, uint8_t *mac){
-  uint32_t res = Hacl_Chacha20Poly1305_aead_decrypt(plaintext,
-                                               ciphertext,
-                                               len,
-                                               mac,
-                                               NULL,
-                                               0,
-                                               k->key,
-                                               k->nonce);
-  return res;
-}
-
-/**
- * Encrypt a message, no AAD for now
- * if res == 0 everything OK
- */
-uint32_t gec_encrypt(struct gec_sym_key *k, uint8_t *ciphertext, uint8_t *plaintext, uint8_t len, uint8_t *mac) {
-  uint32_t res = Hacl_Chacha20Poly1305_aead_encrypt(ciphertext,  // ciphertext
-                                               mac,  // mac
-                                               plaintext,  // plaintext
-                                               len,  // plaintext len
-                                               NULL,  // aad
-                                               0,  // aad len
-                                               k->key,  // key
-                                               k->nonce);  // nonce
-  return res;
-}
-
-
diff --git a/sw/airborne/modules/datalink/gec/gec.h b/sw/airborne/modules/datalink/gec/gec.h
index a4b7d7cf08..51120abcf5 100644
--- a/sw/airborne/modules/datalink/gec/gec.h
+++ b/sw/airborne/modules/datalink/gec/gec.h
@@ -34,50 +34,75 @@
 #include "../ext/hacl-c/Hacl_Curve25519.h"
 #include "../ext/hacl-c/Hacl_SHA2_512.h"
 #include "../ext/hacl-c/Hacl_Chacha20Poly1305.h"
+#include "../ext/hacl-c/kremlib.h"
 
-
+#define PPRZ_MSG_TYPE_PLAINTEXT 0xaa
+#define PPRZ_MSG_TYPE_ENCRYPTED 0x55
+// index of encrypted/payload byte
+#define PPRZ_GEC_IDX 0
+// index of the beginning of the counter
+#define PPRZ_CNTR_IDX 1
+// index of the beginning of the authenticated bytes
+#define PPRZ_AUTH_IDX 5
+// index of the beginning of the ciphertex
+#define PPRZ_CIPH_IDX 7
+// legth of the message signature
 #define PPRZ_SIGN_LEN 64
+// lenght of a hash for key derivation
+#define PPRZ_HASH_LEN 64
+// length of the encryption keys
 #define PPRZ_KEY_LEN 32
-#define PPRZ_NONCE_LEN 12
+// length of the authenticated data
+#define PPRZ_AUTH_LEN 2
+// length of the message authentication tag
 #define PPRZ_MAC_LEN 16
+// length of the message nonce
+#define PPRZ_NONCE_LEN 12
+// length of the counter
+#define PPRZ_COUNTER_LEN 4
+// index of the message ID for plaintext messages
+#define PPRZ_PLAINTEXT_MSG_ID_IDX 4
+// 4 bytes of MSG info (source_ID, dest_ID, class_byte, msg_ID) + 1 GEC byte
+#define PPRZ_PLAINTEXT_MSG_MIN_LEN 5
+// 20 bytes crypto overhead + 4 bytes MSG info + 1 GEC byte
+#define PPRZ_ENCRYPTED_MSG_MIN_LEN 25
+// length of the crypto overhead (4 bytes of counter + 16 bytes of tag)
+#define PPRZ_CRYPTO_OVERHEAD 20
+// basepoint value for the scalar curve multiplication
+#define PPRZ_CURVE_BASEPOINT 9
+// minimal size of the encrypted message
+#define PPRZ_V2_MSG_ID 3
 
 typedef unsigned char ed25519_signature[64];
 
-struct gec_privkey
-{
+struct gec_privkey {
   uint8_t priv[PPRZ_KEY_LEN];
-  uint8_t pub[PPRZ_KEY_LEN];
+  uint8_t pub[PPRZ_KEY_LEN]; bool ready;
 };
 
-struct gec_pubkey
-{
-  uint8_t pub[PPRZ_KEY_LEN];
+struct gec_pubkey {
+  uint8_t pub[PPRZ_KEY_LEN]; bool ready;
 };
 
-struct gec_sym_key
-{
+struct gec_sym_key {
   uint8_t key[PPRZ_KEY_LEN];
   uint8_t nonce[PPRZ_NONCE_LEN];
-  uint32_t ctr;
+  uint32_t counter; bool ready;
 };
 
-typedef enum
-{
+typedef enum {
   INIT, WAIT_MSG1, WAIT_MSG2, WAIT_MSG3, CRYPTO_OK,
 } stage_t;
 
-typedef enum
-{
+typedef enum {
   INITIATOR, RESPONDER, CLIENT, INVALID_PARTY
 } party_t;
 
-typedef enum
-{
+typedef enum {
   P_AE, P_BE, SIG,
 } gec_sts_msg_type_t;
 
-typedef enum
-{
+typedef enum {
   ERROR_NONE,
   // RESPONDER ERRORS
   MSG1_TIMEOUT_ERROR,
@@ -92,39 +117,30 @@ typedef enum
   MSG3_ENCRYPT_ERROR,
   // BOTH PARTIES
   UNEXPECTED_MSG_TYPE_ERROR,
-  UNEXPECTED_STS_STAGE_ERROR,
-  UNEXPECTED_MSG_ERROR
+  UNEXPECTED_MSG_DATA_ERROR,
+  UNEXPECTED_MSG_ERROR,
 } sts_error_t;
 
 // Intermediate data structure containing information relating to the stage of
 // the STS protocol.
-struct gec_sts_ctx
-{
-  struct gec_pubkey theirPublicKey;
-  struct gec_privkey myPrivateKey;
-  struct gec_pubkey theirPublicKeyEphemeral;
-  struct gec_privkey myPrivateKeyEphemeral;
-  struct gec_sym_key theirSymmetricKey;
-  struct gec_sym_key mySymmetricKey;
+struct gec_sts_ctx {
+  struct gec_pubkey their_public_key;
+  struct gec_privkey my_private_key;
+  struct gec_pubkey their_public_ephemeral;
+  struct gec_privkey my_private_ephemeral;
+  struct gec_sym_key rx_sym_key;
+  struct gec_sym_key tx_sym_key;
   stage_t protocol_stage;
   party_t party;
   sts_error_t last_error;
-  uint32_t counter_err;
+  uint32_t rx_counter_err;
   uint32_t encrypt_err;
   uint32_t decrypt_err;
 };
 
-void gec_sts_init(struct gec_sts_ctx * sts);
-
-void clear_sts(struct gec_sts_ctx * sts);
-
-void generate_ephemeral_keys(struct gec_privkey *sk);
-
-void derive_key_material(struct gec_sts_ctx *sts, uint8_t* z);
-
-uint32_t gec_encrypt(struct gec_sym_key *k, uint8_t *ciphertext,
-    uint8_t *plaintext, uint8_t len, uint8_t *mac);
-uint32_t gec_decrypt(struct gec_sym_key *k, uint8_t *plaintext,
-    uint8_t *ciphertext, uint8_t len, uint8_t *mac);
+void gec_sts_init(struct gec_sts_ctx *sts);
+void gec_clear_sts(struct gec_sts_ctx *sts);
+void gec_generate_ephemeral_keys(struct gec_privkey *sk);
+void gec_derive_key_material(struct gec_sts_ctx *sts, uint8_t *z);
 
 #endif /* SPPRZ_GEC_H */
diff --git a/sw/airborne/modules/datalink/gec_dl.c b/sw/airborne/modules/datalink/gec_dl.c
index 5c63c61618..91cab4e42c 100644
--- a/sw/airborne/modules/datalink/gec_dl.c
+++ b/sw/airborne/modules/datalink/gec_dl.c
@@ -29,10 +29,17 @@
 #include "pprzlink/messages.h"
 #include <string.h> // for memset()
 
+#ifdef GEC_STATUS_LED
+#include "led.h" // for LED indication
+#endif
+
 #if PPRZLINK_DEFAULT_VER != 2
 #error "Secure link is only for Pprzlink v 2.0"
 #endif
 
+void gec_encapsulate_and_send_msg(struct pprzlink_msg *msg, long fd);
+static void gec_transport_init(struct gec_transport *t);
+
 struct gec_transport gec_tp;
 
 #if PERIODIC_TELEMETRY
@@ -41,32 +48,118 @@ struct gec_transport gec_tp;
 static void send_secure_link_info(struct transport_tx *trans, struct link_device *dev)
 {
   pprz_msg_send_SECURE_LINK_STATUS(trans, dev, AC_ID,
-    (uint8_t*)&gec_tp.sts.protocol_stage,
-    (uint8_t*)&gec_tp.sts.last_error,
-    &gec_tp.sts.counter_err,
-    &gec_tp.sts.decrypt_err,
-    &gec_tp.sts.encrypt_err);
+                                   (uint8_t *)&gec_tp.sts.protocol_stage,
+                                   (uint8_t *)&gec_tp.sts.last_error,
+                                   &gec_tp.sts.rx_counter_err,
+                                   &gec_tp.sts.decrypt_err,
+                                   &gec_tp.sts.encrypt_err);
 }
-
 #endif
 
+/**
+ * Convert from network byte order (big endian) to the machine byte order
+ */
+uint32_t gec_bytes_to_counter(uint8_t *bytes)
+{
+  // assume big endian
+  uint32_t x = (bytes[3] << 24) + (bytes[2] << 16) + (bytes[1] << 8) + bytes[0];
+  // now check the appropriate endiannes
+  /* ... for Linux */
+#if defined(__linux__) || defined(__CYGWIN__)
+  return ntohl(x);
+  /* ... generic big-endian fallback code */
+#elif defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+  // the byte order is the same as for network
+  return x;
+  /* ... generic little-endian fallback code */
+#elif defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+  // do a byte swap
+  return htobe32(x);
+  /* ... couldn't determine endian-ness of the target platform */
+#else
+#pragma message "Please define __BYTE_ORDER__!"
+#endif /* defined(__linux__) || ... */
+}
 
-static struct gec_transport * get_trans(struct pprzlink_msg *msg)
+/**
+ * Convert counter to bytes in network byte order
+ */
+void gec_counter_to_bytes(uint32_t n, uint8_t *bytes)
+{
+  //first account for appropriate endiannes
+  /* ... for Linux */
+#if defined(__linux__) || defined(__CYGWIN__)
+  n = htonl(n);
+  /* ... generic big-endian fallback code */
+#elif defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+  // the byte order is the same as for network
+  // do nothing
+  /* ... generic little-endian fallback code */
+#elif defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+  // do a byte swap
+  n = htobe32(n);
+  /* ... couldn't determine endian-ness of the target platform */
+#else
+#pragma message "Error: Please define __BYTE_ORDER__!"
+#endif /* defined(__linux__) || ... */
+
+  bytes[3] = (n >> 24) & 0xFF;
+  bytes[2] = (n >> 16) & 0xFF;
+  bytes[1] = (n >> 8) & 0xFF;
+  bytes[0] = n & 0xFF;
+}
+
+/**
+ * Helper function to get the relevant transport struct
+ */
+static struct gec_transport *get_trans(struct pprzlink_msg *msg)
 {
   return (struct gec_transport *)(msg->trans->impl);
 }
 
-static inline void insert_byte(struct gec_transport *t, const uint8_t byte) {
+/**
+ * Simply insert byte to the message buffer
+ */
+static inline void insert_byte(struct gec_transport *t, const uint8_t byte)
+{
   t->tx_msg[t->tx_msg_idx] = byte;
   t->tx_msg_idx++;
 }
 
+// add a message id to the whitelist
+void gec_add_to_whitelist(struct gec_whitelist *whitelist, uint8_t id)
+{
+  if (whitelist->init && (whitelist->idx < WHITELIST_LEN)) {
+    whitelist->whitelist[whitelist->idx] = id;
+    whitelist->idx++;
 
-static void put_bytes(struct pprzlink_msg *msg,
-                      long fd __attribute__((unused)),
+  } else {
+    memset(whitelist, 0, WHITELIST_LEN);  // erase the whitelist
+    whitelist->init = true;
+    whitelist->idx = 0;
+  }
+}
+
+// return true if given message id is in the whitelist
+bool gec_is_in_the_whitelist(struct gec_whitelist *whitelist, uint8_t id)
+{
+  if (whitelist->init) {
+    for (uint8_t i = 0; i < whitelist->idx; i++) {
+      if (whitelist->whitelist[i] == id) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+/**
+ * Simply put bytes in the message buffer
+ */
+static void put_bytes(struct pprzlink_msg *msg, long fd __attribute__((unused)),
                       enum TransportDataType type __attribute__((unused)),
-                      enum TransportDataFormat format __attribute__((unused)),
-                      const void *bytes, uint16_t len)
+                      enum TransportDataFormat format __attribute__((unused)), const void *bytes,
+                      uint16_t len)
 {
   const uint8_t *b = (const uint8_t *) bytes;
   int i;
@@ -75,154 +168,78 @@ static void put_bytes(struct pprzlink_msg *msg,
   }
 }
 
-
 static void put_named_byte(struct pprzlink_msg *msg,
                            long fd __attribute__((unused)),
                            enum TransportDataType type __attribute__((unused)),
-                           enum TransportDataFormat format __attribute__((unused)),
-                           uint8_t byte, const char *name __attribute__((unused)))
+                           enum TransportDataFormat format __attribute__((unused)), uint8_t byte,
+                           const char *name __attribute__((unused)))
 {
   insert_byte(get_trans(msg), byte);
 }
 
-
 /**
- * TODO
+ * Return the length of a regular pprzlink message (PPRZ_STX .. CHECKSUM)
+ * and add CRYPTO_OVERHEAD(20) + crypto byte(1)
  */
 static uint8_t size_of(struct pprzlink_msg *msg, uint8_t len)
 {
-  // message length: payload + crypto overhead
-  return len + get_trans(msg)->pprz_tp.trans_tx.size_of(msg, len);
-  // + ???; TODO depends on crypto header size
+  return PPRZ_CRYPTO_OVERHEAD + 1
+         + get_trans(msg)->pprz_tp.trans_tx.size_of(msg, len);
 }
 
-
 /**
- * TODO
+ * Just reset the message data
  */
 static void start_message(struct pprzlink_msg *msg,
                           long fd __attribute__((unused)),
-                          uint8_t payload_len)
+                          uint8_t payload_len __attribute__((unused)))
 {
-  PPRZ_MUTEX_LOCK(get_trans(msg)->mtx_tx); // lock mutex
-  memset(get_trans(msg)->tx_msg, _FD, TRANSPORT_PAYLOAD_LEN); // erase message data
-  get_trans(msg)->tx_msg_idx = 0; // reset index
-  // TODO add crypto header to buffer if needed
+  PPRZ_MUTEX_LOCK(get_trans(msg)->mtx_tx);  // lock mutex
+  memset(get_trans(msg)->tx_msg, _FD, TRANSPORT_PAYLOAD_LEN);  // erase message data
+  get_trans(msg)->tx_msg_idx = 0;  // reset index
 }
 
 /**
- * TODO
- */
-static void end_message(struct pprzlink_msg *msg, long fd)
-{
-  // TODO apply crypto on buffer here, or a part of it
-
-  // if valid
-    // encapsulated encrypted data with pprz
-    get_trans(msg)->pprz_tp.trans_tx.start_message(msg, fd, get_trans(msg)->tx_msg_idx);
-    for (int i = 0; i < get_trans(msg)->tx_msg_idx; i++) {
-      // add byte one by one for now
-      get_trans(msg)->pprz_tp.trans_tx.put_bytes(msg, _FD, DL_TYPE_UINT8, DL_FORMAT_SCALAR,
-          &(get_trans(msg)->tx_msg[i]), 1);
-      // TODO: replace with one put_bytes() call
-    }
-    get_trans(msg)->pprz_tp.trans_tx.end_message(msg, fd);
-
-  // unlock mutex
-  PPRZ_MUTEX_UNLOCK(get_trans(msg)->mtx_tx);
-}
-
-
-/**
- * TODO
+ * Report to the underlying transport
  */
 static void overrun(struct pprzlink_msg *msg)
 {
   get_trans(msg)->pprz_tp.trans_tx.overrun(msg);
 }
 
+/**
+ * Call the underlying transport
+ */
 static void count_bytes(struct pprzlink_msg *msg, uint8_t bytes)
 {
   get_trans(msg)->pprz_tp.trans_tx.count_bytes(msg, bytes);
 }
 
-static int check_available_space(struct pprzlink_msg *msg, long *fd, uint16_t bytes)
+/**
+ * Check that there is enought space in the buffer.
+ * Two cases:
+ * if CRYPTO_OK:
+ *  return the check_available_space from the underlying transport
+ * else:
+ *  return zero, to avoid race conditions over the message buffer (no messages are being
+ *  sent before the key exchange)
+ */
+static int check_available_space(struct pprzlink_msg *msg, long *fd,
+                                 uint16_t bytes)
 {
-  return get_trans(msg)->pprz_tp.trans_tx.check_available_space(msg, fd, bytes);
-}
-
-
-// Init pprz transport structure
-static void gec_transport_init(struct gec_transport *t)
-{
-  t->trans_rx.msg_received = false;
-  t->trans_tx.size_of = (size_of_t) size_of;
-  t->trans_tx.check_available_space = (check_available_space_t) check_available_space;
-  t->trans_tx.put_bytes = (put_bytes_t) put_bytes;
-  t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte;
-  t->trans_tx.start_message = (start_message_t) start_message;
-  t->trans_tx.end_message = (end_message_t) end_message;
-  t->trans_tx.overrun = (overrun_t) overrun;
-  t->trans_tx.count_bytes = (count_bytes_t) count_bytes;
-  t->trans_tx.impl = (void *)(t);
-  PPRZ_MUTEX_INIT(t->mtx_tx); // init mutex, check if correct pointer
-}
-
-
-
-
-
-
-void gec_process_sts_msg(struct link_device *dev, struct gec_transport *trans, uint8_t *buf)
-{
-  uint8_t sender_id = SenderIdOfPprzMsg(buf);
-  uint8_t msg_id = IdOfPprzMsg(buf);
-
-  if (sender_id != 0) {
-    // process only messages from GCS
-    // log an error
-    return;
-  }
-
-  uint8_t msg_type = DL_KEY_EXCHANGE_GCS_msg_type(buf);
-
-  switch (msg_id) {
-    case DL_KEY_EXCHANGE_GCS:
-      switch (trans->sts.protocol_stage) {
-        case WAIT_MSG1:
-          if (msg_type == P_AE) {
-            respond_sts(dev, trans, buf);
-          } else {
-            // log an error
-            trans->sts.last_error = UNEXPECTED_MSG_TYPE_ERROR;
-          }
-          break;
-        case WAIT_MSG3:
-          if (msg_type == SIG) {
-            finish_sts(dev, trans, buf);
-          } else {
-            // log an error
-            trans->sts.last_error = UNEXPECTED_MSG_TYPE_ERROR;
-          }
-          break;
-        default:
-          // INIT, WAIT_MSG2, CRYPTO_OK
-          // do nothing or log an error
-          trans->sts.last_error = UNEXPECTED_STS_STAGE_ERROR;
-          break;
-      }
-      break;
-    default:
-      // process only KEY_EXCHANGE for now
-      // log an error
-      trans->sts.last_error = UNEXPECTED_MSG_ERROR;
-      break;
+  if (get_trans(msg)->sts.protocol_stage == CRYPTO_OK) {
+    return get_trans(msg)->pprz_tp.trans_tx.check_available_space(msg, fd,
+           bytes);
   }
+  return 0;
 }
 
-
 void gec_dl_init(void)
 {
+#ifdef GEC_STATUS_LED
+  LED_OFF(GEC_STATUS_LED);
+#endif
+
   // init pprz transport
   pprz_transport_init(&gec_tp.pprz_tp);
 
@@ -237,136 +254,464 @@ void gec_dl_init(void)
 #endif
 }
 
+/**
+ * Get bytes from the message buffer (SENDER_ID .. MSG_PAYLOAD)
+ * Add crypto byte
+ * Attempt encryption
+ * If succesfull, add counter, auth, ciphertext and tag (CRYPTO_BYTE .. TAG)
+ * and wrap in pprz transport (PPRZ_STX .. CHECKSUM)
+ */
+static void end_message(struct pprzlink_msg *msg, long fd)
+{
+  switch (gec_tp.sts.protocol_stage) {
+    case CRYPTO_OK:
+      if (gec_encrypt_message(gec_tp.tx_msg, &gec_tp.tx_msg_idx)) {
+        gec_encapsulate_and_send_msg(msg, fd);
+      }
+      break;
+    default:
+      // shouldn't be here as sending messages is not allowed until after the key exchange
+      break;
+  }
+  // unlock mutex
+  PPRZ_MUTEX_UNLOCK(get_trans(msg)->mtx_tx);
+}
 
+// Init pprz transport structure
+static void gec_transport_init(struct gec_transport *t)
+{
+  t->trans_rx.msg_received = false;
+  t->trans_tx.size_of = (size_of_t) size_of;
+  t->trans_tx.check_available_space =
+    (check_available_space_t) check_available_space;
+  t->trans_tx.put_bytes = (put_bytes_t) put_bytes;
+  t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte;
+  t->trans_tx.start_message = (start_message_t) start_message;
+  t->trans_tx.end_message = (end_message_t) end_message;
+  t->trans_tx.overrun = (overrun_t) overrun;
+  t->trans_tx.count_bytes = (count_bytes_t) count_bytes;
+  t->trans_tx.impl = (void *)(t);
+  PPRZ_MUTEX_INIT(t->mtx_tx);  // init mutex, check if correct pointer
+
+  // add whitelist messages
+  gec_add_to_whitelist(&(t->whitelist), KEY_EXCHANGE_MSG_ID_UAV);
+  gec_add_to_whitelist(&(t->whitelist), KEY_EXCHANGE_MSG_ID_GCS);
+}
+
+/**
+ * Encapsulate message data (CRYPTO_BYTE .. MSG_PAYLOAD .. TAG) with
+ * pprzlink header and checksum (PPRZ_STX .. CHECKSUM).
+ * and send message
+ */
+void gec_encapsulate_and_send_msg(struct pprzlink_msg *msg, long fd)
+{
+  get_trans(msg)->pprz_tp.trans_tx.start_message(msg, fd,
+      get_trans(msg)->tx_msg_idx);
+  get_trans(msg)->pprz_tp.trans_tx.put_bytes(msg, _FD, DL_TYPE_UINT8,
+      DL_FORMAT_SCALAR, get_trans(msg)->tx_msg, get_trans(msg)->tx_msg_idx);
+  get_trans(msg)->pprz_tp.trans_tx.end_message(msg, fd);
+}
+
+/**
+ * Attempts message encryption
+ * Adds crypto_byte, counter and tag
+ * Assumes that the first two bytes of 'payload' are source_ID and dest_ID
+ * and these two bytes will be authenticated
+ * Returns encrypted pprzlink 2.0 message (crypto_byte .. tag) in the same
+ * buffer as was the incoming message
+ */
+bool gec_encrypt_message(uint8_t *buf, uint8_t *payload_len)
+{
+  // check payload length
+  if (*payload_len <= PPRZ_V2_MSG_ID) {
+    return false;
+  }
+  // check if the key is ready
+  if (!gec_tp.sts.tx_sym_key.ready) {
+    return false;
+  }
+
+  // increment counter
+  uint32_t counter = gec_tp.sts.tx_sym_key.counter + 1;
+  // convert to bytes
+  uint8_t counter_as_bytes[4];
+  gec_counter_to_bytes(counter, counter_as_bytes);
+  // update nonce
+  memcpy(gec_tp.sts.tx_sym_key.nonce, counter_as_bytes, sizeof(uint32_t));
+
+  // update intermediate fields
+  uint8_t auth[PPRZ_AUTH_LEN];
+  memcpy(auth, buf, PPRZ_AUTH_LEN);
+  uint8_t tag[PPRZ_MAC_LEN];
+  uint8_t ciphertext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
+  uint32_t mlen = *payload_len - PPRZ_AUTH_LEN;  // at least CLASS_BYTE and MSG_ID
+
+  uint32_t res = Hacl_Chacha20Poly1305_aead_encrypt(ciphertext, tag,
+                 &buf[PPRZ_AUTH_LEN], mlen, auth,
+                 PPRZ_AUTH_LEN, gec_tp.sts.tx_sym_key.key, gec_tp.sts.tx_sym_key.nonce);
+
+  if (res == 0) {
+    memset(buf, 0, TRANSPORT_PAYLOAD_LEN);  // reset  whole buffer
+    *payload_len = 0;
+    buf[PPRZ_GEC_IDX] = PPRZ_MSG_TYPE_ENCRYPTED;  // CRYPTO_BYTE
+    *payload_len += 1;
+    memcpy(&buf[PPRZ_CNTR_IDX], counter_as_bytes, sizeof(uint32_t));  // counter
+    *payload_len += sizeof(uint32_t);
+    memcpy(&buf[PPRZ_AUTH_IDX], auth, PPRZ_AUTH_LEN);  // auth data
+    *payload_len += PPRZ_AUTH_LEN;
+    memcpy(&buf[PPRZ_CIPH_IDX], ciphertext, mlen);  // ciphertext
+    *payload_len += mlen;
+    memcpy(&buf[PPRZ_CIPH_IDX + mlen], tag, PPRZ_MAC_LEN);  // tag
+    *payload_len += PPRZ_MAC_LEN;
+    gec_tp.sts.tx_sym_key.counter = counter;  // update counter
+    return true;
+  } else {
+    gec_tp.sts.encrypt_err++;
+    return false;
+  }
+
+}
+
+/**
+ * Attemp message decryption
+ * If a message is unencrypted, pass it through only if the MSG_ID is in the whitelist
+ * Returns Pprzlink 2.0 message bytes (source_ID .. msg payload)
+ *
+ * Input: expects (CRYPTO_BYTE .. MSG_DATA .. optional TAG)
+ * Output: returns stripped message (SENDER_ID .. MSG_PAYLOAD)
+ */
+bool gec_decrypt_message(uint8_t *buf, volatile uint8_t *payload_len)
+{
+  switch (buf[PPRZ_GEC_IDX]) {
+    case PPRZ_MSG_TYPE_PLAINTEXT:
+      // handle plaintext message
+      if (*payload_len < PPRZ_PLAINTEXT_MSG_MIN_LEN) {
+        return false;
+      }
+      // check if the message is in the whitelist
+      if (gec_is_in_the_whitelist(&gec_tp.whitelist,
+                                  buf[PPRZ_PLAINTEXT_MSG_ID_IDX])) {
+        // return the buffer minus the crypto byte
+        memmove(buf, &buf[1], *payload_len - 1);
+        return true;
+      }
+      return false;
+      break;
+    case PPRZ_MSG_TYPE_ENCRYPTED:
+      // attempt decryption
+      if (*payload_len < PPRZ_ENCRYPTED_MSG_MIN_LEN) {
+        return false;
+      }
+      if (!gec_tp.sts.rx_sym_key.ready) {
+        // the rx key is not ready yet
+        return false;
+      }
+      // first check the message counter
+      uint32_t counter = gec_bytes_to_counter(&buf[PPRZ_CNTR_IDX]);
+      // check against the saved counter
+      if (counter <= gec_tp.sts.rx_sym_key.counter) {
+        gec_tp.sts.rx_counter_err++;
+        return false;
+      }
+      // update nonce with 4 counter bytes
+      memcpy(gec_tp.sts.rx_sym_key.nonce, &buf[PPRZ_CNTR_IDX],
+             sizeof(uint32_t));
+      // update intermediate fields
+      uint8_t auth[PPRZ_AUTH_LEN];
+      memcpy(auth, &buf[PPRZ_AUTH_IDX], PPRZ_AUTH_LEN);
+      uint8_t tag[PPRZ_MAC_LEN];
+      memcpy(tag, &buf[*payload_len - PPRZ_MAC_LEN], PPRZ_MAC_LEN);
+      uint8_t ciphertext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
+      // payload - AUTH_DATA(2) - TAG(16) - CRYPTO_BYTE(1) - COUNTER(4) = payload_len - 23
+      uint32_t mlen = *payload_len - PPRZ_CRYPTO_OVERHEAD - 1 - PPRZ_AUTH_LEN;  // don't forget to count for CRYPTO_BYTE
+      memcpy(ciphertext, &buf[PPRZ_CIPH_IDX], mlen);
+      uint8_t plaintext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
+
+      // try decryption
+      uint32_t res = Hacl_Chacha20Poly1305_aead_decrypt(plaintext, ciphertext,
+                     mlen, tag, auth,
+                     PPRZ_AUTH_LEN, gec_tp.sts.rx_sym_key.key,
+                     gec_tp.sts.rx_sym_key.nonce);
+
+      // res == 0 means all good
+      if (res == 0) {
+        memset(buf, 0, TRANSPORT_PAYLOAD_LEN);  // reset  whole buffer
+        *payload_len = 0;
+        memcpy(buf, auth, PPRZ_AUTH_LEN);  // copy auth data
+        *payload_len += PPRZ_AUTH_LEN;
+        memcpy(&buf[PPRZ_AUTH_LEN], plaintext, mlen);  // copy plaintext
+        *payload_len += mlen;
+        gec_tp.sts.rx_sym_key.counter = counter;  // update counter
+        return true;
+      } else {
+        gec_tp.sts.decrypt_err++;
+        return false;
+      }
+      break;
+    default:
+      break;
+  }
+  return false;
+}
+
+/**
+ * Parse incoming message bytes (PPRZ_STX..CHCKSUM B) and returns a new decrypted message if it is available.
+ * While the status != Crypto_OK no message is returned, and all logic is handled internally.
+ * Returned message has format of
+ * Pprzlink 2.0
+ * ```ignore
+ * payload[0] source SENDER_ID
+ * payload[1] destination ID
+ * payload[2] class/component
+ * payload[3] MSG_ID
+ * payload[4-end] MSG_PAYLOAD
+ * ```
+ * and can be directly processed by a parser
+ *
+ * NOTE: the KEY_EXCHANGE messages are whitelisted and thus pass through the "decryption" even if they are
+ * in plaintext and after the key exchange is established. This is not a problem, because we don't act on them.
+ * It would be good to limit "sending" them from other processes, but for now lets assume nobody will want to send
+ * this message.
+ *
+ * In the future, the KEY_EXCHANGE messages will be handled in `firmware_parse_datalink_msgs()` and act upon accordingly
+ * (i.e. the key renegotiation will be possible). For now, I am keeping this note here as a reminder.
+ */
 void gec_dl_event(void)
 {
-  pprz_check_and_parse(&DOWNLINK_DEVICE.device, &gec_tp.pprz_tp, gec_tp.trans_rx.payload,
-                      (bool*) &gec_tp.trans_rx.msg_received);
-  if (gec_tp.trans_rx.msg_received) {
-    if (gec_tp.sts.protocol_stage != CRYPTO_OK) {
-      // message is in plaintext, treat is as such
-      // TODO: check for decryption
-      // don't pass through
-      gec_process_sts_msg(&DOWNLINK_DEVICE.device, &gec_tp, gec_tp.trans_rx.payload);
+  // strip PPRZ_STX, MSG_LEN, and CHEKSUM, return (CRYPTO_BYTE..MSG_PAYLOAD)
+  pprz_check_and_parse(&DOWNLINK_DEVICE.device, &gec_tp.pprz_tp,
+                       gec_tp.pprz_tp.trans_rx.payload, (bool *) &gec_tp.trans_rx.msg_received);
+  // we have (CRYPTO_BYTE .. MSG_PAYLOAD) in
+  if (gec_tp.trans_rx.msg_received) {  // self.rx.parse_byte(b)
 
-    } else {
-      // attempt decryption, pass data if all good
-      // process decrypted message if needed
-      // store in dl_buffer (see macro in datalink.h)
-      DatalinkFillDlBuffer(gec_tp.trans_rx.payload, TRANSPORT_PAYLOAD_LEN);
-
-      // pass to datalink
-      DlCheckAndParse(&DOWNLINK_DEVICE.device, &gec_tp.trans_tx, dl_buffer, &dl_msg_available);
+    switch (gec_tp.sts.protocol_stage) {
+      case CRYPTO_OK:
+        // decrypt message
+        // if successfull return the message (sender_ID .. MSG_payload)
+        if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
+                                &gec_tp.pprz_tp.trans_rx.payload_len)) {
+          // copy the buffer over
+          // NOTE/TODO: the real payload_len is at least one byte shorter
+          // but we can copy whole buffer anyway since we don't overflow
+          DatalinkFillDlBuffer(gec_tp.pprz_tp.trans_rx.payload,
+                               gec_tp.pprz_tp.trans_rx.payload_len);
+          // pass to datalink
+          DlCheckAndParse(&DOWNLINK_DEVICE.device, &gec_tp.trans_tx, dl_buffer,
+                          &dl_msg_available);
+        }
+        break;
+      case WAIT_MSG1:
+        // decrypt message (checks against the whitelist and removes the crypto byte)
+        // returns (SENDER_ID .. MSG_PAYLOAD
+        if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
+                                &gec_tp.pprz_tp.trans_rx.payload_len)) {
+          gec_process_msg1(gec_tp.pprz_tp.trans_rx.payload);
+        } else {
+        }
+        break;
+      case WAIT_MSG3:
+        // decrypt message (checks against the whitelist and removes the crypto byte)
+        // returns (SENDER_ID .. MSG_PAYLOAD
+        if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
+                                &gec_tp.pprz_tp.trans_rx.payload_len)) {
+          if (gec_process_msg3(gec_tp.pprz_tp.trans_rx.payload)) {
+            // if OK, we are ready for an ongoing communication
+            gec_tp.sts.protocol_stage = CRYPTO_OK;
+#ifdef GEC_STATUS_LED
+            LED_ON(GEC_STATUS_LED);
+#endif
+          }
+        }
+        break;
+      default:
+        // shouldn't be here unless something is really wrong
+        break;
     }
     // reset flag
     gec_tp.trans_rx.msg_received = false;
   }
 }
 
-
 /**
- * message1 = [ Pae (32 bytes) ]
- * 2. B generates ephemeral curve25519 key pair (Pbe, Qbe).
- * 3. B computes the shared secret: z = scalar_multiplication(Qbe, Pae)
- * 4. B uses the key derivation function kdf(z,1) to compute Kb || Sb,
- * kdf(z,0) to compute Ka || Sa, and kdf(z,2) to compute Kclient || Sclient.
- * 5. B computes the ed25519 signature: sig = signQb(Pbe || Pae)
- * 6. B computes and sends the message Pbe || Ekey=Kb,IV=Sb||zero(sig)
- *
- * Sends: message2 = [ Pbe (32 bytes) | Encrypted Signature (64 bytes) ] + MAC (16bytes)
+ * NOTE: for RESPONDER party only
+ *  Process incoming message (expected MSG1)
+ *  if the right (KEY_EXCHANGE) message received with the right data (P_AE)
+ *  and the right P_AE.len=PPRZ_KEY_LEN, the internal state of Sts gets updated
+ *  (key derivation etc), and msg2 is prepared to be sent.
+ *  Input: decrypted message (source_ID .. msg payload)
+ *  If all good, it sends response message
  */
-void respond_sts(struct link_device *dev, struct gec_transport *trans, uint8_t *buf) {
-  // TODO: check message length
+void gec_process_msg1(uint8_t *buf)
+{
+  if (SenderIdOfPprzMsg(buf) != 0) {
+    // process only messages from GCS
+    // log an error
+    return;
+  }
+
+  // check message ID
+  if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
+    gec_tp.sts.last_error = UNEXPECTED_MSG_ERROR;
+    return;
+  }
+
+  // check message type
+  if (DL_KEY_EXCHANGE_GCS_msg_type(buf) != P_AE) {
+    gec_tp.sts.last_error = UNEXPECTED_MSG_TYPE_ERROR;
+    return;
+  }
+
+  // check data length
+  if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf) != PPRZ_KEY_LEN) {
+    gec_tp.sts.last_error = UNEXPECTED_MSG_DATA_ERROR;
+    return;
+  }
 
   // copy P_ae over
-  memcpy(trans->sts.theirPublicKeyEphemeral.pub, DL_KEY_EXCHANGE_GCS_msg_data(buf), sizeof(struct gec_pubkey));
+  memcpy(&gec_tp.sts.their_public_ephemeral.pub,
+         DL_KEY_EXCHANGE_GCS_msg_data(buf), sizeof(struct gec_pubkey));
+  gec_tp.sts.their_public_ephemeral.ready = true;
 
   // 2. B generates ephemeral curve25519 key pair (Pbe, Qbe).
-  generate_ephemeral_keys(&trans->sts.myPrivateKeyEphemeral);
+  if (!gec_tp.sts.my_private_ephemeral.ready) {
+    gec_generate_ephemeral_keys(&gec_tp.sts.my_private_ephemeral);
+  }
 
   // 3. B computes the shared secret: z = scalar_multiplication(Qbe, Pae)
-  uint8_t z[32] = {0};
-  Hacl_Curve25519_crypto_scalarmult(z, trans->sts.myPrivateKeyEphemeral.priv, trans->sts.theirPublicKeyEphemeral.pub);
+  uint8_t z[32] = { 0 };
+  Hacl_Curve25519_crypto_scalarmult(z, gec_tp.sts.my_private_ephemeral.priv,
+                                    gec_tp.sts.their_public_ephemeral.pub);
 
   // 4. B uses the key derivation function kdf(z,1) to compute Kb || Sb,
   // kdf(z,0) to compute Ka || Sa, and kdf(z,2) to compute Kclient || Sclient.
-  derive_key_material(&trans->sts, z);
+  gec_derive_key_material(&gec_tp.sts, z);
 
   // 5. B computes the ed25519 signature: sig = signQb(Pbe || Pae)
-  uint8_t sig[PPRZ_SIGN_LEN] = {0};
-  uint8_t pbe_concat_p_ae[PPRZ_KEY_LEN*2] = {0};
-  memcpy(pbe_concat_p_ae, trans->sts.myPrivateKeyEphemeral.pub, PPRZ_KEY_LEN);
-  memcpy(&pbe_concat_p_ae[PPRZ_KEY_LEN], trans->sts.theirPublicKeyEphemeral.pub, PPRZ_KEY_LEN);
-  Hacl_Ed25519_sign(sig, trans->sts.myPrivateKey.priv, pbe_concat_p_ae, PPRZ_KEY_LEN*2);
+  uint8_t sig[PPRZ_SIGN_LEN] = { 0 };
+  uint8_t pbe_concat_p_ae[PPRZ_KEY_LEN * 2] = { 0 };
+  memcpy(pbe_concat_p_ae, &gec_tp.sts.my_private_ephemeral.pub, PPRZ_KEY_LEN);
+  memcpy(&pbe_concat_p_ae[PPRZ_KEY_LEN], &gec_tp.sts.their_public_ephemeral.pub,
+         PPRZ_KEY_LEN);
+  Hacl_Ed25519_sign(sig, gec_tp.sts.my_private_key.priv, pbe_concat_p_ae,
+                    PPRZ_KEY_LEN * 2);
 
   // 6. B computes and sends the message Pbe || Ekey=Kb,IV=Sb||zero(sig)
-  uint8_t msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN + PPRZ_MAC_LEN] = {0};
-  memcpy(msg_data, &trans->sts.myPrivateKeyEphemeral.pub, PPRZ_KEY_LEN);
-  if (gec_encrypt(&trans->sts.mySymmetricKey, &msg_data[PPRZ_KEY_LEN], sig, PPRZ_SIGN_LEN, &msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN])) {
-    // log error here and return
-    trans->sts.last_error = MSG1_ENCRYPT_ERROR;
+  uint8_t msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN + PPRZ_MAC_LEN] = { 0 };
+  memcpy(msg_data, &gec_tp.sts.my_private_ephemeral.pub, PPRZ_KEY_LEN);
+
+  if (Hacl_Chacha20Poly1305_aead_encrypt(&msg_data[PPRZ_KEY_LEN],
+                                         &msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN], sig, PPRZ_SIGN_LEN, NULL, 0,
+                                         gec_tp.sts.tx_sym_key.key, gec_tp.sts.tx_sym_key.nonce)) {
+    gec_tp.sts.last_error = MSG1_ENCRYPT_ERROR;
     return;
   }
-  // all good, send message and increment status
-  uint8_t msg_type = P_BE;
-  uint8_t nb_msg_data = PPRZ_KEY_LEN + PPRZ_SIGN_LEN + PPRZ_MAC_LEN;
 
+  // now we have to manually construct the message
+  // CRYPTO BYTE
+  // source_id
+  // sender_id
+  // class id
+  // msg id
+  // msg_type
+  // msg_data
+  gec_tp.tx_msg[PPRZ_GEC_IDX] = PPRZ_MSG_TYPE_PLAINTEXT;
+  gec_tp.tx_msg_idx = 1;
+  gec_tp.tx_msg[gec_tp.tx_msg_idx] = AC_ID;
+  gec_tp.tx_msg_idx++;
+  gec_tp.tx_msg[gec_tp.tx_msg_idx] = 0;
+  gec_tp.tx_msg_idx++;
+  gec_tp.tx_msg[gec_tp.tx_msg_idx] = 1;  // telemetry
+  gec_tp.tx_msg_idx++;
+  gec_tp.tx_msg[gec_tp.tx_msg_idx] = 239;
+  gec_tp.tx_msg_idx++;
+  gec_tp.tx_msg[gec_tp.tx_msg_idx] = P_BE;
+  gec_tp.tx_msg_idx++;
+  gec_tp.tx_msg[gec_tp.tx_msg_idx] = sizeof(msg_data);
+  gec_tp.tx_msg_idx++;
+  memcpy(&gec_tp.tx_msg[gec_tp.tx_msg_idx], msg_data, sizeof(msg_data));
+  gec_tp.tx_msg_idx += sizeof(msg_data);
 
-  pprz_msg_send_KEY_EXCHANGE_UAV(&trans->trans_tx, dev, AC_ID, &msg_type, nb_msg_data, msg_data);
+  struct pprzlink_msg msg2;
+  msg2.trans = &gec_tp.pprz_tp.trans_tx;
+  msg2.dev = &DOWNLINK_DEVICE.device;
 
-  // update protocol stage
-  trans->sts.protocol_stage = WAIT_MSG3;
-  // TODO: add timeout
+  gec_encapsulate_and_send_msg(&msg2, 0);
+
+  /*
+   * Note that ideally we would use the following function call, but the problem is that
+   * 'end_message()` cannot currently distinguish between encrypted and plaintext message to be sent
+   * (i.e. I don't know the message ID). The second problem I am using the same buffer here as for
+   * constructing a message, so a corruption of the buffer is possible.
+   *
+   * Since I am doing this only once, the method above is fine for the time being.
+   *
+   uint8_t msg_status = P_BE;
+   pprz_msg_send_KEY_EXCHANGE_UAV(&gec_tp.pprz_tp.trans_tx, &DOWNLINK_DEVICE.device, AC_ID,
+   &msg_status, sizeof(msg_data), msg_data);
+   */
+
+  // increment status
+  gec_tp.sts.protocol_stage = WAIT_MSG3;
 }
 
-
-
 /**
- * Finalize STS exchange
- *
- * message3 = [ Encrypted Signature (64 bytes) ] + MAC(16 bytes) = Ekey=Ka,IV=Sa||zero(sig)
- * where sig = signQa(Pae || Pbe)
- * 13. B decrypts the message and verifies the signature.
+ * Process incoming message (expected MSG3)
+ *  if the right (KEY_EXCHANGE) message received with the right data (SIG)
+ *  and the right SIG.len=PPRZ_SIGN_LEN, and the signature is verified, Ok() is returned.
+ *  Input: decrypted message (source_ID .. msg payload)
+ *  Returns true if this party is ready for communication
  */
-void finish_sts(struct link_device *dev __attribute__((__unused__)),
-                struct gec_transport *trans,
-                uint8_t *buf){
-  // TODO: check message length
+bool gec_process_msg3(uint8_t *buf)
+{
+  if (SenderIdOfPprzMsg(buf) != 0) {
+    // process only messages from GCS
+    // log an error
+    return false;
+  }
 
-  // decrypt
-  uint8_t sign[PPRZ_SIGN_LEN] = {0};
-  if (gec_decrypt(&trans->sts.theirSymmetricKey, sign, DL_KEY_EXCHANGE_GCS_msg_data(buf),
-      PPRZ_SIGN_LEN, DL_KEY_EXCHANGE_GCS_msg_data(buf)+PPRZ_SIGN_LEN)) {
-    // log error and return
-    // MSG3 decrypt error
-    trans->sts.last_error = MSG3_DECRYPT_ERROR;
-    return;
+// check message ID
+  if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
+    gec_tp.sts.last_error = UNEXPECTED_MSG_ERROR;
+    return false;
+  }
+
+// check message type
+  if (DL_KEY_EXCHANGE_GCS_msg_type(buf) != SIG) {
+    gec_tp.sts.last_error = UNEXPECTED_MSG_TYPE_ERROR;
+    return false;
+  }
+
+// check data length
+  if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf)
+      != (PPRZ_SIGN_LEN + PPRZ_MAC_LEN)) {
+    gec_tp.sts.last_error = UNEXPECTED_MSG_DATA_ERROR;
+    return false;
+  }
+
+  // 13. B decrypts the message and verifies the signature.
+  uint8_t sign[PPRZ_SIGN_LEN] = { 0 };
+  if (Hacl_Chacha20Poly1305_aead_decrypt(sign,
+                                         DL_KEY_EXCHANGE_GCS_msg_data(buf),
+                                         PPRZ_SIGN_LEN, DL_KEY_EXCHANGE_GCS_msg_data(buf) + PPRZ_SIGN_LEN, NULL, 0,
+                                         gec_tp.sts.rx_sym_key.key, gec_tp.sts.rx_sym_key.nonce)) {
+    gec_tp.sts.last_error = MSG3_DECRYPT_ERROR;
+    return false;
   }
 
   // verify
-  uint8_t pbe_concat_p_ae[PPRZ_KEY_LEN*2] = {0};
-  memcpy(pbe_concat_p_ae, trans->sts.myPrivateKeyEphemeral.pub, PPRZ_KEY_LEN);
-  memcpy(&pbe_concat_p_ae[PPRZ_KEY_LEN], trans->sts.theirPublicKeyEphemeral.pub, PPRZ_KEY_LEN);
+  uint8_t p_ae_concat_p_be[PPRZ_KEY_LEN * 2] = { 0 };
+  memcpy(p_ae_concat_p_be, &gec_tp.sts.their_public_ephemeral.pub,
+         PPRZ_KEY_LEN);
+  memcpy(&p_ae_concat_p_be[PPRZ_KEY_LEN], &gec_tp.sts.my_private_ephemeral.pub,
+         PPRZ_KEY_LEN);
   // returns true if verified properly
-  if (!Hacl_Ed25519_verify(trans->sts.theirPublicKey.pub, pbe_concat_p_ae, PPRZ_SIGN_LEN, sign)) {
+  if (!Hacl_Ed25519_verify(gec_tp.sts.their_public_key.pub, p_ae_concat_p_be,
+                           PPRZ_SIGN_LEN, sign)) {
     // log error and return
-    // MSG3 sign verify error
-    trans->sts.last_error = MSG3_SIGNVERIFY_ERROR;
-    return;
+    gec_tp.sts.last_error = MSG3_SIGNVERIFY_ERROR;
+    return false;
   }
 
- /*
-  // update the transport
-  // tx key => my symm key
-  memcpy(trans->tx_key, sts.mySymmetricKey.key, PPRZ_KEY_LEN);
-  // tx nonce (12 bytes, first 4 bytes will be overwritten by the counter)
-  memcpy(trans->tx_nonce, sts.mySymmetricKey.nonce, PPRZ_NONCE_LEN);
-
-  // rx key => their symm key
-  memcpy(trans->rx_key, sts.theirSymmetricKey.key, PPRZ_KEY_LEN);
-  // rx nonce (12 bytes, first 4 bytes will be overwritten by the counter)
-  memcpy(trans->rx_nonce, sts.theirSymmetricKey.nonce, PPRZ_NONCE_LEN);
-*/
-  // if everything went OK, proceed to CRYPTO_OK status
-  trans->sts.protocol_stage = CRYPTO_OK;
+  // all ok
+  return true;
 }
diff --git a/sw/airborne/modules/datalink/gec_dl.h b/sw/airborne/modules/datalink/gec_dl.h
index 48d5545992..5cc984440b 100644
--- a/sw/airborne/modules/datalink/gec_dl.h
+++ b/sw/airborne/modules/datalink/gec_dl.h
@@ -39,6 +39,33 @@
 #include "mcu_periph/udp.h"
 #endif
 
+// TODO: a hack to to allow passing the key exchage messages before
+// secure comlink is established
+#define KEY_EXCHANGE_MSG_ID_UAV 239
+#define KEY_EXCHANGE_MSG_ID_GCS 159
+#define WHITELIST_LEN 20
+
+/**
+ * Whitelist for sending and receiving
+ * unencrypted messages
+ * (mostly for KEY_EXCHANGE messages)
+ */
+struct gec_whitelist {
+  uint8_t whitelist[WHITELIST_LEN];
+  uint8_t idx;
+  bool init;
+};
+
+/**
+ * Auxilliary message buffer for storing
+ * precomputed key exchange messages
+ */
+struct message_buffer {
+  uint8_t msg[TRANSPORT_PAYLOAD_LEN];
+  uint8_t len;
+  bool message_ready;
+};
+
 struct gec_transport {
   // pprz encapsulation layer
   struct pprz_transport pprz_tp;
@@ -50,15 +77,16 @@ struct gec_transport {
   struct transport_tx trans_tx;
   // buffered tx message
   uint8_t tx_msg[TRANSPORT_PAYLOAD_LEN];
-  volatile uint8_t tx_msg_idx;
+  uint8_t tx_msg_idx;
 
   // ecnryption primitives
   struct gec_sts_ctx sts;
 
+  struct gec_whitelist whitelist;
+
   PPRZ_MUTEX(mtx_tx); // optional mutex
 };
 
-
 /** PPRZ transport structure */
 extern struct gec_transport gec_tp;
 
@@ -70,7 +98,7 @@ extern void gec_dl_event(void);
 
 /** Parsing a frame data and copy the payload to the datalink buffer */
 void gec_check_and_parse(struct link_device *dev, struct gec_transport *trans,
-                          uint8_t *buf, bool *msg_available);
+                         uint8_t *buf, bool *msg_available);
 
 void gec_process_sts_msg(struct link_device *dev, struct gec_transport *trans, uint8_t *buf);
 
@@ -78,5 +106,14 @@ void respond_sts(struct link_device *dev, struct gec_transport *trans, uint8_t *
 
 void finish_sts(struct link_device *dev, struct gec_transport *trans, uint8_t *buf);
 
+void gec_add_to_whitelist(struct gec_whitelist *whitelist, uint8_t id);
+bool gec_is_in_the_whitelist(struct gec_whitelist *whitelist, uint8_t id);
+bool gec_decrypt_message(uint8_t *buf, volatile uint8_t *payload_len);
+bool gec_encrypt_message(uint8_t *buf, uint8_t *payload_len);
+void gec_counter_to_bytes(uint32_t n, uint8_t *bytes);
+uint32_t gec_bytes_to_counter(uint8_t *bytes);
+void gec_process_msg1(uint8_t *buf);
+bool gec_process_msg3(uint8_t *buf);
+
 #endif /* GEC_DL_H */
 
diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h
index 48c1eddcd0..2f6888b381 100644
--- a/sw/airborne/subsystems/datalink/datalink.h
+++ b/sw/airborne/subsystems/datalink/datalink.h
@@ -66,6 +66,7 @@ EXTERN bool datalink_enabled;
 #endif
 
 /** Convenience macro to fill dl_buffer */
+// TODO: replace with a memcpy for efficiency
 #define DatalinkFillDlBuffer(_buf, _len) { \
   uint16_t _i = 0; \
   for (_i = 0; _i < _len; _i++) { \
diff --git a/sw/ext/rustlink b/sw/ext/rustlink
index 1e70ae72d4..508a6d3fa1 160000
--- a/sw/ext/rustlink
+++ b/sw/ext/rustlink
@@ -1 +1 @@
-Subproject commit 1e70ae72d456feee980aa1db04dc5728e225b0e2
+Subproject commit 508a6d3fa11d7defabb142fd2fc9fa4430817599