Crypto updates (#2245)

* Cleaned up GEC code

* Added airborne support for pprzlink 1.0

* Update hacl-c

* Add proper indexing for pprz messages

* Updated airframe config for secure link

* Code style changes and updated code style script

* Bump rustlink

* Compilation fixes

conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml

@@ -14,6 +14,10 @@ Aggie Air ARK
       </module>
       <configure name="HAS_LUFTBOOT" value="1"/>
       <configure name="FLASH_MODE" value="DFU"/>
+      <module name="telemetry"   type="transparent">
+        <configure name="MODEM_PORT"        value="UART3"/>
+        <configure name="MODEM_BAUD"        value="B57600"/>
+      </module>
     </target>
 
     <!-- NOTE: if you want to use extra_dl module for HITL
@@ -24,9 +28,9 @@ Aggie Air ARK
     <target name="nps" board="pc">
       <module name="fdm" type="jsbsim"/>
       <module name="radio_control" type="spektrum"/>
+      <module name="telemetry"   type="transparent"/>
     </target>
 
-<!-- hitl currently not supported for secure link comms
     <target name="hitl" board="pc">
       <module name="fdm" type="jsbsim"/>
       <module name="imu" type="nps"/>
@@ -35,19 +39,14 @@ Aggie Air ARK
       <configure name="INS_BAUD" value="B921600"/>
       <configure name="AP_DEV" value="/dev/ttyUSB2"/>
       <configure name="AP_BAUD" value="B921600"/>
+      <module name="telemetry"   type="transparent"/>
     </target>
--->
 
     <module name="motor_mixing"/>
     <module name="actuators"     type="pwm">
       <define name="SERVO_HZ" value="160"/>
     </module>
 
-    <module name="telemetry"     type="transparent_gec">
-      <configure name="MODEM_PORT" value="UART3"/>
-      <configure name="MODEM_BAUD" value="B57600"/>
-    </module>
-
     <module name="stabilization" type="float_euler"/>
 
     <module name="ins"       type="vectornav">
@@ -58,7 +57,6 @@ Aggie Air ARK
 
   <modules>
     <module name="sys_mon"/>
-    <!--module name="rng"/-->
     <module name="copilot"/>
     <module name="extra_dl">
       <!-- in order to use uart1 without chibios we need to remap the peripheral-->

conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml

@@ -19,17 +19,14 @@
       <configure name="FLASH_MODE" value="DFU"/>
     </target>
 
+    <!-- NOTE: if you want to use extra_dl module for HITL
+    you have to set TELEMETRY_FREQUENCY to PERIODIC_FREQUENCY -->
     <configure name="PERIODIC_FREQUENCY" value="160"/>
-
+    <configure name="TELEMETRY_FREQUENCY" value="160"/>
 
     <target name="nps" board="pc">
       <module name="fdm" type="jsbsim"/>
       <module name="radio_control" type="spektrum"/>
-      <!--configure name="USE_HITL" value="1"/-->
-      <configure name="INS_DEV" value="/dev/ttyUSB1"/>
-      <configure name="INS_BAUD" value="B921600"/>
-      <configure name="AP_DEV" value="/dev/ttyUSB2"/>
-      <configure name="AP_BAUD" value="B921600"/>
     </target>
 
     <module name="stabilization" type="int_quat"/>
@@ -64,12 +61,12 @@
     <module name="lidar_lite">
       <configure name="LIDAR_LITE_I2C_DEV" value="i2c2"/>
     </module>
-    <!--module name="battery_monitor.xml">
+    <module name="battery_monitor.xml">
       <define name="BATMON_CURRENT_OFFSET" value="-120"/>
       <define name="BATMON_CURRENT_SENSITIVITY" value="25.6"/>
       <define name="BATMON_TEMP_OFFSET" value="250"/>
       <define name="BATMON_TEMP_SENSITIVITY" value="10"/>
-    </module-->
+    </module>
   </modules>
 
     <servos driver="Pwm">

conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml

@@ -62,6 +62,7 @@ AggieAir Atomic Tangerine
     <module name="nav" type="survey_poly_osam"/>
     <module name="nav" type="launcher"/>
     <module name="nav" type="skid_landing"/>
+
     <module name="sys_mon"/>
     <module name="copilot"/>
     <module name="extra_dl">
@@ -236,9 +237,9 @@ AggieAir Atomic Tangerine
     <!-- ORIGINAL GAIN     -->
     <define name="PITCH_DGAIN" value="1.5"/>
 
-    <!-- SIMULATOR GAIN
+    <!-- SIMULATOR GAIN-->
-    <define name="PITCH_DGAIN" value="151.5"/>
+    <!--define name="PITCH_DGAIN" value="151.5"/-->
-     -->
+     
     <define name="ELEVATOR_OF_ROLL" value="1250"/>
     <define name="ROLL_ATTITUDE_GAIN" value="9000"/>
     <define name="ROLL_RATE_GAIN" value="1600"/>

conf/airframes/AGGIEAIR/aggieair_conf.xml

@@ -7,7 +7,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
    settings="settings/rotorcraft_basic.xml settings/nps.xml"
-   settings_modules="modules/battery_monitor.xml modules/gps.xml modules/stabilization_float_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml"
+   settings_modules="modules/battery_monitor.xml modules/gps.xml modules/stabilization_float_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml"
    gui_color="#ffff954c0000"
   />
   <aircraft
@@ -18,7 +18,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
    settings="settings/rotorcraft_basic.xml settings/nps.xml"
-   settings_modules="modules/lidar_lite.xml modules/gps.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml"
+   settings_modules="modules/battery_monitor.xml modules/lidar_lite.xml modules/gps.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml"
    gui_color="#ffff954c0000"
   />
   <aircraft
@@ -40,7 +40,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_fixedwing.xml"
    flight_plan="flight_plans/AGGIEAIR/BasicTuning_Launcher.xml"
    settings="settings/fixedwing_basic.xml settings/nps.xml"
-   settings_modules="modules/battery_monitor.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"
+   settings_modules="modules/battery_monitor.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 modules/imu_common.xml"
    gui_color="#00009e93ffff"
   />
   <aircraft

conf/airframes/AGGIEAIR/aggieair_control_panel.xml

@@ -22,6 +22,7 @@
       <arg flag="-a" constant="@AC_ID"/>
       <arg flag="-n" constant="@AIRCRAFT"/>
       <arg flag="-c" />
+      <arg flag="-v" constant="2.0"/>
     </program>
     <program name="Link Combiner" command="sw/ground_segment/python/redundant_link/link_combiner.py"/>
     <program name="GCS" command="sw/ground_segment/cockpit/gcs"/>
@@ -129,6 +130,7 @@
        <arg flag="-a" constant="@AC_ID"/>
        <arg flag="-n" constant="@AIRCRAFT"/>
        <arg flag="-c" />
+       <arg flag="-v" constant="2.0"/>
      </program>
      <program name="Server">
       <arg flag="-n"/>
@@ -177,6 +179,32 @@
      </program>
    </session>
 
+   <session name="Secure HITL 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" />
+       <arg flag="-v" constant="2.0"/>
+     </program>
+     <program name="Server">
+      <arg flag="" constant="-no_md5_check"/>
+     </program>
+     <program name="GCS">
+      <arg flag="-layout" constant="AGGIEAIR/vertical.xml"/>
+     </program>
+     <program name="Messages"/>
+     <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="-t" constant="hitl"/>
+      <arg flag="--nodisplay"/>
+     </program>
+   </session>
+
    <session name="HITL USB-serial@115200">
      <program name="Data Link">
        <arg flag="-d" constant="/dev/ttyUSB0"/>

conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml

@@ -11,8 +11,8 @@ AggieAir RP3 Minion
       <module name="radio_control" type="sbus">
         <configure name="SBUS_PORT" value="UART5"/>
       </module>
-      <configure name="HAS_LUFTBOOT" value="0"/>
+      <configure name="HAS_LUFTBOOT" value="1"/>
-      <configure name="FLASH_MODE" value="JTAG"/>
+      <configure name="FLASH_MODE" value="DFU"/>
     </target>
 
     <!-- NOTE: if you want to use extra_dl module for HITL
@@ -60,8 +60,7 @@ AggieAir RP3 Minion
     <module name="nav" type="survey_poly_osam"/>
     <module name="nav" type="launcher"/>
     <module name="nav" type="skid_landing"/>
-    <module name="rng"/>
+
-    <module name="haclc"/>
     <module name="sys_mon"/>
     <module name="copilot"/>
     <module name="extra_dl">

conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml

@@ -21,7 +21,7 @@
       <message name="DATALINK_REPORT"     period="5.1"/>
       <message name="DL_VALUE"            period="1.5"/>
       <message name="IR_SENSORS"          period="1.2"/>
-      <message name="SURVEY"              period="2.1"/>
+      <message name="SECURE_LINK_STATUS"  period="2.1"/>
       <message name="COMMANDS"            period="1"/>
       <message name="FBW_STATUS"          period="2"/>
       <message name="VECTORNAV_INFO"      period="1.0"/>

fix_code_style.sh

@@ -11,6 +11,7 @@ if [ $# -eq 0 ]; then
 fi
 
 ASTYLE_VERSION=`astyle --version 2>&1| awk '{print $4}'`
+ASTYLE_VERSION=${ASTYLE_VERSION:0:4}
 echo "Using astyle version $ASTYLE_VERSION"
 
 set -f

sw/airborne/modules/datalink/gec/gec.c

@@ -26,6 +26,10 @@
  */
 #include "modules/datalink/gec/gec.h"
 
+#if defined(__linux__) || defined(__CYGWIN__)
+#include <arpa/inet.h>
+#endif
+
 void gec_sts_init(struct gec_sts_ctx *sts)
 {
   // reset all keys
@@ -75,27 +79,26 @@ void gec_generate_ephemeral_keys(struct gec_privkey *sk)
     sk->priv[i + 2] = (uint8_t)(tmp >> 16);
     sk->priv[i + 3] = (uint8_t)(tmp >> 24);
   }
-  uint8_t basepoint[32] = {0};
+  uint8_t basepoint[32] = { 0 };
-  basepoint[0] = 9; // default basepoint
+  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 gec_derive_key_material(struct gec_sts_ctx *ctx, uint8_t *z)
 {
-  uint8_t tmp[PPRZ_KEY_LEN * 2] = {0};
+  uint8_t tmp[PPRZ_KEY_LEN * 2] = { 0 };
-  uint8_t input[PPRZ_KEY_LEN + 1] = {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->rx_sym_key.key, tmp, PPRZ_KEY_LEN); // K_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
+  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;
 
@@ -108,3 +111,55 @@ void gec_derive_key_material(struct gec_sts_ctx *ctx, uint8_t *z)
   ctx->tx_sym_key.ready = true;
 }
 
+/**
+ * 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__) || ... */
+}
+
+/**
+ * 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;
+}

sw/airborne/modules/datalink/gec/gec.h

@@ -21,7 +21,7 @@
 /**
  * @file datalink/gec/gec.h
  *
- * Galois embedded crypto iplementation
+ * Galois embedded crypto implementation
  *
  */
 #ifndef SPPRZ_GEC_H
@@ -44,34 +44,51 @@
 #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
-// 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
+// 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
+
+#if PPRZLINK_DEFAULT_VER == 2
+// minimal size of the encrypted message
+#define PPRZ_MSG_ID 3
 // 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)
+// length of the authenticated data (SOURCE ID, DEST ID)
-#define PPRZ_CRYPTO_OVERHEAD 20
+#define PPRZ_AUTH_LEN 2
-// basepoint value for the scalar curve multiplication
+// index of the beginning of the ciphertext
-#define PPRZ_CURVE_BASEPOINT 9
+#define PPRZ_CIPH_IDX 7
+#else // PPRZLINK_DEFAULT_VER = 1
 // minimal size of the encrypted message
-#define PPRZ_V2_MSG_ID 3
+#define PPRZ_MSG_ID 1
+// index of the message ID for plaintext messages
+#define PPRZ_PLAINTEXT_MSG_ID_IDX 2
+// 2 bytes of MSG info (source_ID, msg_ID) + 1 GEC byte
+#define PPRZ_PLAINTEXT_MSG_MIN_LEN 3
+// 20 bytes crypto overhead + 2 bytes MSG info + 1 GEC byte
+#define PPRZ_ENCRYPTED_MSG_MIN_LEN 23
+// length of the authenticated data (SOURCE ID)
+#define PPRZ_AUTH_LEN 1
+// index of the beginning of the ciphertext
+#define PPRZ_CIPH_IDX 6
+#endif
 
 typedef unsigned char ed25519_signature[64];
 
@@ -143,4 +160,7 @@ 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);
 
+void gec_counter_to_bytes(uint32_t n, uint8_t *bytes);
+uint32_t gec_bytes_to_counter(uint8_t *bytes);
+
 #endif /* SPPRZ_GEC_H */

sw/airborne/modules/datalink/gec_dl.c

@@ -33,13 +33,6 @@
 #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
@@ -56,67 +49,6 @@ static void send_secure_link_info(struct transport_tx *trans, struct link_device
 }
 #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__) || ... */
-}
-
-/**
- * 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);
-}
-
 /**
  * Simply insert byte to the message buffer
  */
@@ -126,31 +58,15 @@ static inline void insert_byte(struct gec_transport *t, const uint8_t byte)
   t->tx_msg_idx++;
 }
 
-// add a message id to the whitelist
+#if PPRZLINK_DEFAULT_VER == 2
-void gec_add_to_whitelist(struct gec_whitelist *whitelist, uint8_t id)
+void gec_encapsulate_and_send_msg(struct pprzlink_msg *msg, long fd);
-{
-  if (whitelist->init && (whitelist->idx < WHITELIST_LEN)) {
-    whitelist->whitelist[whitelist->idx] = id;
-    whitelist->idx++;
 
-  } else {
+/**
-    memset(whitelist, 0, WHITELIST_LEN);  // erase the whitelist
+ * Helper function to get the relevant transport struct
-    whitelist->init = true;
+ */
-    whitelist->idx = 0;
+static struct gec_transport *get_trans(struct pprzlink_msg *msg)
-  }
-}
-
-// 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) {
+  return (struct gec_transport *)(msg->trans->impl);
-    for (uint8_t i = 0; i < whitelist->idx; i++) {
-      if (whitelist->whitelist[i] == id) {
-        return true;
-      }
-    }
-  }
-  return false;
 }
 
 /**
@@ -195,8 +111,8 @@ static void start_message(struct pprzlink_msg *msg,
                           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
+  memset(get_trans(msg)->tx_msg, _FD, TRANSPORT_PAYLOAD_LEN);// erase message data
-  get_trans(msg)->tx_msg_idx = 0;  // reset index
+  get_trans(msg)->tx_msg_idx = 0;// reset index
 }
 
 /**
@@ -234,26 +150,6 @@ static int check_available_space(struct pprzlink_msg *msg, long *fd,
   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);
-
-  // init crypto transport
-  gec_transport_init(&gec_tp);
-
-  // initialize keys
-  gec_sts_init(&gec_tp.sts);
-
-#if PERIODIC_TELEMETRY
-  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SECURE_LINK_STATUS, send_secure_link_info);
-#endif
-}
-
 /**
  * Get bytes from the message buffer (SENDER_ID .. MSG_PAYLOAD)
  * Add crypto byte
@@ -277,8 +173,117 @@ static void end_message(struct pprzlink_msg *msg, long fd)
   PPRZ_MUTEX_UNLOCK(get_trans(msg)->mtx_tx);
 }
 
+/**
+ * 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);
+}
+
+#else // Pprzlink 1.0
+
+void gec_encapsulate_and_send_msg(struct gec_transport *trans,
+                                  struct link_device *dev, long fd);
+
+static void put_bytes(struct gec_transport *trans,
+                      struct link_device *dev __attribute__((unused)),
+                      long fd __attribute__((unused)),
+                      enum TransportDataType type __attribute__((unused)),
+                      enum TransportDataFormat format __attribute__((unused)), const void *bytes,
+                      uint16_t len)
+{
+  const uint8_t *b = (const uint8_t *) bytes;
+  int i;
+  for (i = 0; i < len; i++) {
+    insert_byte(trans, b[i]);
+  }
+}
+
+static void put_named_byte(struct gec_transport *trans,
+                           struct link_device *dev __attribute__((unused)),
+                           long fd __attribute__((unused)),
+                           enum TransportDataType type __attribute__((unused)),
+                           enum TransportDataFormat format __attribute__((unused)), uint8_t byte,
+                           const char *name __attribute__((unused)))
+{
+  insert_byte(trans, byte);
+}
+
+static uint8_t size_of(struct gec_transport *trans __attribute__((unused)),
+                       uint8_t len)
+{
+  return PPRZ_CRYPTO_OVERHEAD + 1 + trans->pprz_tp.trans_tx.size_of(trans, len);
+}
+
+static void start_message(struct gec_transport *trans,
+                          struct link_device *dev __attribute__((unused)),
+                          long fd __attribute__((unused)),
+                          uint8_t payload_len __attribute__((unused)))
+{
+  PPRZ_MUTEX_LOCK(trans->mtx_tx);  // lock mutex
+  memset(trans->tx_msg, _FD, TRANSPORT_PAYLOAD_LEN);  // erase message data
+  trans->tx_msg_idx = 0;  // reset index
+}
+
+static void end_message(struct gec_transport *trans, struct link_device *dev,
+                        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(trans, dev, fd);
+      }
+      break;
+    default:
+      // shouldn't be here as sending messages is not allowed until after the key exchange
+      break;
+  }
+  // unlock mutex
+  PPRZ_MUTEX_UNLOCK(trans->mtx_tx);
+}
+
+void gec_encapsulate_and_send_msg(struct gec_transport *trans,
+                                  struct link_device *dev, long fd)
+{
+  trans->pprz_tp.trans_tx.start_message(trans, dev, fd, trans->tx_msg_idx);
+  trans->pprz_tp.trans_tx.put_bytes(trans, dev, _FD, DL_TYPE_UINT8,
+                                    DL_FORMAT_SCALAR, trans->tx_msg, trans->tx_msg_idx);
+  trans->pprz_tp.trans_tx.end_message(trans, dev, fd);
+}
+
+static void overrun(struct gec_transport *trans __attribute__((unused)),
+                    struct link_device *dev)
+{
+  trans->pprz_tp.trans_tx.overrun(trans, dev);
+}
+
+static void count_bytes(struct gec_transport *trans __attribute__((unused)),
+                        struct link_device *dev, uint8_t bytes)
+{
+  trans->pprz_tp.trans_tx.count_bytes(trans, dev, bytes);
+}
+
+static int check_available_space(
+  struct gec_transport *trans __attribute__((unused)),
+  struct link_device *dev, long *fd, uint16_t bytes)
+{
+  if (trans->sts.protocol_stage == CRYPTO_OK) {
+    return trans->pprz_tp.trans_tx.check_available_space(trans, dev, fd, bytes);
+  }
+  return 0;
+}
+
+#endif // PPRZLINK 2.0/1.0
+
 // Init pprz transport structure
-static void gec_transport_init(struct gec_transport *t)
+void gec_transport_init(struct gec_transport *t)
 {
   t->trans_rx.msg_received = false;
   t->trans_tx.size_of = (size_of_t) size_of;
@@ -298,32 +303,36 @@ static void gec_transport_init(struct gec_transport *t)
   gec_add_to_whitelist(&(t->whitelist), KEY_EXCHANGE_MSG_ID_GCS);
 }
 
-/**
+void gec_dl_init(void)
- * 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,
+#ifdef GEC_STATUS_LED
-      get_trans(msg)->tx_msg_idx);
+  LED_OFF(GEC_STATUS_LED);
-  get_trans(msg)->pprz_tp.trans_tx.put_bytes(msg, _FD, DL_TYPE_UINT8,
+#endif
-      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);
+  // init pprz transport
+  pprz_transport_init(&gec_tp.pprz_tp);
+
+  // init crypto transport
+  gec_transport_init(&gec_tp);
+
+  // initialize keys
+  gec_sts_init(&gec_tp.sts);
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SECURE_LINK_STATUS, send_secure_link_info);
+#endif
 }
 
 /**
  * Attempts message encryption
  * Adds crypto_byte, counter and tag
- * Assumes that the first two bytes of 'payload' are source_ID and dest_ID
+ * Returns encrypted pprzlink message (crypto_byte .. tag) in the same
- * 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) {
+  if (*payload_len <= PPRZ_MSG_ID) {
     return false;
   }
   // check if the key is ready
@@ -369,13 +378,12 @@ bool gec_encrypt_message(uint8_t *buf, uint8_t *payload_len)
     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)
+ * Returns Pprzlink message bytes (source_ID .. msg payload)
  *
  * Input: expects (CRYPTO_BYTE .. MSG_DATA .. optional TAG)
  * Output: returns stripped message (SENDER_ID .. MSG_PAYLOAD)
@@ -466,6 +474,13 @@ bool gec_decrypt_message(uint8_t *buf, volatile uint8_t *payload_len)
  * payload[3] MSG_ID
  * payload[4-end] MSG_PAYLOAD
  * ```
+ * or
+ * Pprzlink 1.0
+ * ```ignore
+ * payload[0] source SENDER_ID
+ * payload[1] MSG_ID
+ * payload[2-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
@@ -473,7 +488,7 @@ bool gec_decrypt_message(uint8_t *buf, volatile uint8_t *payload_len)
  * 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
+ * In the future, the KEY_EXCHANGE messages should be handled even in CRYPTO_OK mode and acted 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)
@@ -491,7 +506,7 @@ void gec_dl_event(void)
         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
+          // NOTE: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);
@@ -606,6 +621,7 @@ void gec_process_msg1(uint8_t *buf)
     return;
   }
 
+#if PPRZLINK_DEFAULT_VER == 2
   // now we have to manually construct the message
   // CRYPTO BYTE
   // source_id
@@ -620,7 +636,7 @@ void gec_process_msg1(uint8_t *buf)
   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[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++;
@@ -636,6 +652,29 @@ void gec_process_msg1(uint8_t *buf)
   msg2.dev = &DOWNLINK_DEVICE.device;
 
   gec_encapsulate_and_send_msg(&msg2, 0);
+#else
+  // Pprzlink 1.0
+  // now we have to manually construct the message
+  // CRYPTO BYTE
+  // source_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] = 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);
+
+  gec_encapsulate_and_send_msg(&gec_tp, &DOWNLINK_DEVICE.device, 0);
+#endif // PPRZLINK 1.0/2.0
 
   /*
    * Note that ideally we would use the following function call, but the problem is that
@@ -645,6 +684,7 @@ void gec_process_msg1(uint8_t *buf)
    *
    * Since I am doing this only once, the method above is fine for the time being.
    *
+   * Pprzlink 2.0:
    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);
@@ -715,3 +755,30 @@ bool gec_process_msg3(uint8_t *buf)
   // all ok
   return true;
 }
+
+// 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++;
+
+  } 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;
+}

sw/airborne/modules/datalink/gec_dl.h

@@ -39,8 +39,8 @@
 #include "mcu_periph/udp.h"
 #endif
 
-// TODO: a hack to to allow passing the key exchage messages before
+// MSG_ID for key exchange messages.
-// secure comlink is established
+// Update if IDs change in messages.xml
 #define KEY_EXCHANGE_MSG_ID_UAV 239
 #define KEY_EXCHANGE_MSG_ID_GCS 159
 #define WHITELIST_LEN 20
@@ -81,10 +81,10 @@ struct gec_transport {
 
   // ecnryption primitives
   struct gec_sts_ctx sts;
-
   struct gec_whitelist whitelist;
 
-  PPRZ_MUTEX(mtx_tx); // optional mutex
+  // optional mutex
+  PPRZ_MUTEX(mtx_tx);
 };
 
 /** PPRZ transport structure */
@@ -96,6 +96,8 @@ extern void gec_dl_init(void);
 /** Datalink Event */
 extern void gec_dl_event(void);
 
+void gec_transport_init(struct gec_transport *t);
+
 /** 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);
@@ -103,15 +105,14 @@ void gec_check_and_parse(struct link_device *dev, struct gec_transport *trans,
 void gec_process_sts_msg(struct link_device *dev, struct gec_transport *trans, uint8_t *buf);
 
 void respond_sts(struct link_device *dev, struct gec_transport *trans, uint8_t *buf);
-
+void finish_sts(struct link_device *dev, struct gec_transport *trans,
-void finish_sts(struct link_device *dev, struct gec_transport *trans, uint8_t *buf);
+                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);