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
-    <define name="PITCH_DGAIN" value="151.5"/>
-     -->
+    <!-- SIMULATOR GAIN-->
+    <!--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="FLASH_MODE" value="JTAG"/>
+      <configure name="HAS_LUFTBOOT" value="1"/>
+      <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};
-  basepoint[0] = 9; // default basepoint
+  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 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};
+  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->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.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;
 
@@ -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)
-#define PPRZ_CRYPTO_OVERHEAD 20
-// basepoint value for the scalar curve multiplication
-#define PPRZ_CURVE_BASEPOINT 9
+// length of the authenticated data (SOURCE ID, DEST ID)
+#define PPRZ_AUTH_LEN 2
+// index of the beginning of the ciphertext
+#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.h

@@ -39,8 +39,8 @@
 #include "mcu_periph/udp.h"
 #endif
 
-// TODO: a hack to to allow passing the key exchage messages before
-// secure comlink is established
+// MSG_ID for key exchange messages.
+// 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, uint8_t *buf);
+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);