Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
gec_dl.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
26 #include "gec_dl.h"
29 #include "pprzlink/messages.h"
30 #include <string.h> // for memset()
31 
32 #if PPRZLINK_DEFAULT_VER == 2
33 // minimal size of the encrypted message
34 #define PPRZ_MSG_ID 3
35 // index of the message ID for plaintext messages
36 #define PPRZ_PLAINTEXT_MSG_ID_IDX 4
37 // 4 bytes of MSG info (source_ID, dest_ID, class_byte, msg_ID) + 1 GEC byte
38 #define PPRZ_PLAINTEXT_MSG_MIN_LEN 5
39 // 20 bytes crypto overhead + 4 bytes MSG info + 1 GEC byte
40 #define PPRZ_ENCRYPTED_MSG_MIN_LEN 25
41 // length of the authenticated data (SOURCE ID, DEST ID)
42 #define PPRZ_AUTH_LEN 2
43 // index of the beginning of the ciphertext
44 #define PPRZ_CIPH_IDX 7
45 #else
46 #if PPRZLINK_DEFAULT_VER == 1
47 // minimal size of the encrypted message
48 #define PPRZ_MSG_ID 1
49 // index of the message ID for plaintext messages
50 #define PPRZ_PLAINTEXT_MSG_ID_IDX 2
51 // 2 bytes of MSG info (source_ID, msg_ID) + 1 GEC byte
52 #define PPRZ_PLAINTEXT_MSG_MIN_LEN 3
53 // 20 bytes crypto overhead + 2 bytes MSG info + 1 GEC byte
54 #define PPRZ_ENCRYPTED_MSG_MIN_LEN 23
55 // length of the authenticated data (SOURCE ID)
56 #define PPRZ_AUTH_LEN 1
57 // index of the beginning of the ciphertext
58 #define PPRZ_CIPH_IDX 6
59 #endif // PPRZLINK_DEFAULT_VER = 1
60 #endif // PPRZLINK_DEFAULT_VER = 2
61 
62 #ifdef GEC_STATUS_LED
63 #include "led.h" // for LED indication
64 #endif
65 
66 #ifndef GEC_UPDATE_DL
67 #define GEC_UPDATE_DL TRUE
68 #endif
69 
70 struct gec_transport gec_tp;
71 
72 #if PERIODIC_TELEMETRY
74 
75 static void send_secure_link_info(struct transport_tx *trans, struct link_device *dev)
76 {
77  pprz_msg_send_SECURE_LINK_STATUS(trans, dev, AC_ID,
83 }
84 #endif
85 
89 static inline void insert_byte(struct gec_transport *t, const uint8_t byte)
90 {
91  t->tx_msg[t->tx_msg_idx] = byte;
92  t->tx_msg_idx++;
93 }
94 
95 #if PPRZLINK_DEFAULT_VER == 2
96 void gec_encapsulate_and_send_msg(struct pprzlink_msg *msg, long fd);
97 
101 static struct gec_transport *get_trans(struct pprzlink_msg *msg)
102 {
103  return (struct gec_transport *)(msg->trans->impl);
104 }
105 
109 static void put_bytes(struct pprzlink_msg *msg, long fd __attribute__((unused)),
110  enum TransportDataType type __attribute__((unused)),
111  enum TransportDataFormat format __attribute__((unused)), const void *bytes,
112  uint16_t len)
113 {
114  const uint8_t *b = (const uint8_t *) bytes;
115  int i;
116  for (i = 0; i < len; i++) {
117  insert_byte(get_trans(msg), b[i]);
118  }
119 }
120 
121 static void put_named_byte(struct pprzlink_msg *msg,
122  long fd __attribute__((unused)),
123  enum TransportDataType type __attribute__((unused)),
124  enum TransportDataFormat format __attribute__((unused)), uint8_t byte,
125  const char *name __attribute__((unused)))
126 {
127  insert_byte(get_trans(msg), byte);
128 }
129 
134 static uint8_t size_of(struct pprzlink_msg *msg, uint8_t len)
135 {
136  return PPRZ_CRYPTO_OVERHEAD + 1
137  + get_trans(msg)->pprz_tp.trans_tx.size_of(msg, len);
138 }
139 
143 static void start_message(struct pprzlink_msg *msg,
144  long fd __attribute__((unused)),
145  uint8_t payload_len __attribute__((unused)))
146 {
147  PPRZ_MUTEX_LOCK(get_trans(msg)->mtx_tx); // lock mutex
148  memset(get_trans(msg)->tx_msg, 0, TRANSPORT_PAYLOAD_LEN);// erase message data
149  get_trans(msg)->tx_msg_idx = 0;// reset index
150 }
151 
155 static void overrun(struct pprzlink_msg *msg)
156 {
157  get_trans(msg)->pprz_tp.trans_tx.overrun(msg);
158 }
159 
163 static void count_bytes(struct pprzlink_msg *msg, uint8_t bytes)
164 {
165  get_trans(msg)->pprz_tp.trans_tx.count_bytes(msg, bytes);
166 }
167 
177 static int check_available_space(struct pprzlink_msg *msg, long *fd,
178  uint16_t bytes)
179 {
180  if (get_trans(msg)->sts.protocol_stage == CRYPTO_OK) {
181  return get_trans(msg)->pprz_tp.trans_tx.check_available_space(msg, fd,
182  bytes);
183  }
184  return 0;
185 }
186 
194 static void end_message(struct pprzlink_msg *msg, long fd)
195 {
196  switch (gec_tp.sts.protocol_stage) {
197  case CRYPTO_OK:
199  gec_encapsulate_and_send_msg(msg, fd);
200  return;
201  }
202  break;
203  default:
204  // shouldn't be here as sending messages is not allowed until after the key exchange
205  break;
206  }
207  // unlock mutex
208  PPRZ_MUTEX_UNLOCK(get_trans(msg)->mtx_tx);
209 }
210 
216 void gec_encapsulate_and_send_msg(struct pprzlink_msg *msg, long fd)
217 {
218  get_trans(msg)->pprz_tp.trans_tx.start_message(msg, fd, get_trans(msg)->tx_msg_idx);
219  get_trans(msg)->pprz_tp.trans_tx.put_bytes(msg, fd, DL_TYPE_UINT8,
220  DL_FORMAT_SCALAR, get_trans(msg)->tx_msg, get_trans(msg)->tx_msg_idx);
221  // unlock mutex
222  PPRZ_MUTEX_UNLOCK(get_trans(msg)->mtx_tx);
223  get_trans(msg)->pprz_tp.trans_tx.end_message(msg, fd);
224 }
225 
226 #else
227 #if PPRZLINK_DEFAULT_VER == 1
228 void gec_encapsulate_and_send_msg(struct gec_transport *trans,
229  struct link_device *dev, long fd);
230 
231 static void put_bytes(struct gec_transport *trans,
232  struct link_device *dev __attribute__((unused)),
233  long fd __attribute__((unused)),
234  enum TransportDataType type __attribute__((unused)),
235  enum TransportDataFormat format __attribute__((unused)), const void *bytes,
236  uint16_t len)
237 {
238  const uint8_t *b = (const uint8_t *) bytes;
239  int i;
240  for (i = 0; i < len; i++) {
241  insert_byte(trans, b[i]);
242  }
243 }
244 
245 static void put_named_byte(struct gec_transport *trans,
246  struct link_device *dev __attribute__((unused)),
247  long fd __attribute__((unused)),
248  enum TransportDataType type __attribute__((unused)),
249  enum TransportDataFormat format __attribute__((unused)), uint8_t byte,
250  const char *name __attribute__((unused)))
251 {
252  insert_byte(trans, byte);
253 }
254 
255 static uint8_t size_of(struct gec_transport *trans __attribute__((unused)),
256  uint8_t len)
257 {
258  return PPRZ_CRYPTO_OVERHEAD + 1 + trans->pprz_tp.trans_tx.size_of(trans, len);
259 }
260 
261 static void start_message(struct gec_transport *trans,
262  struct link_device *dev __attribute__((unused)),
263  long fd __attribute__((unused)),
264  uint8_t payload_len __attribute__((unused)))
265 {
266  PPRZ_MUTEX_LOCK(trans->mtx_tx); // lock mutex
267  memset(trans->tx_msg, 0, TRANSPORT_PAYLOAD_LEN); // erase message data
268  trans->tx_msg_idx = 0; // reset index
269 }
270 
271 static void end_message(struct gec_transport *trans, struct link_device *dev,
272  long fd)
273 {
274  switch (gec_tp.sts.protocol_stage) {
275  case CRYPTO_OK:
277  gec_encapsulate_and_send_msg(trans, dev, fd);
278  return;
279  }
280  break;
281  default:
282  // shouldn't be here as sending messages is not allowed until after the key exchange
283  break;
284  }
285  // unlock mutex
286  PPRZ_MUTEX_UNLOCK(trans->mtx_tx);
287 }
288 
289 void gec_encapsulate_and_send_msg(struct gec_transport *trans,
290  struct link_device *dev, long fd)
291 {
292  trans->pprz_tp.trans_tx.start_message(trans, dev, fd, trans->tx_msg_idx);
293  trans->pprz_tp.trans_tx.put_bytes(trans, dev, fd, DL_TYPE_UINT8,
294  DL_FORMAT_SCALAR, trans->tx_msg, trans->tx_msg_idx);
295  // unlock mutex
296  PPRZ_MUTEX_UNLOCK(trans->mtx_tx);
297  trans->pprz_tp.trans_tx.end_message(trans, dev, fd);
298 }
299 
300 static void overrun(struct gec_transport *trans __attribute__((unused)),
301  struct link_device *dev)
302 {
303  trans->pprz_tp.trans_tx.overrun(trans, dev);
304 }
305 
306 static void count_bytes(struct gec_transport *trans __attribute__((unused)),
307  struct link_device *dev, uint8_t bytes)
308 {
309  trans->pprz_tp.trans_tx.count_bytes(trans, dev, bytes);
310 }
311 
312 static int check_available_space(
313  struct gec_transport *trans __attribute__((unused)),
314  struct link_device *dev, long *fd, uint16_t bytes)
315 {
316  if (trans->sts.protocol_stage == CRYPTO_OK) {
317  return trans->pprz_tp.trans_tx.check_available_space(trans, dev, fd, bytes);
318  }
319  return 0;
320 }
321 #endif // PPRZLINK_DEFAULT_VER == 1
322 #endif // PPRZLINK_DEFAULT_VER == 2
323 
324 // Init pprz transport structure
326 {
327  t->trans_rx.msg_received = false;
328  t->trans_tx.size_of = (size_of_t) size_of;
329  t->trans_tx.check_available_space =
330  (check_available_space_t) check_available_space;
331  t->trans_tx.put_bytes = (put_bytes_t) put_bytes;
332  t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte;
333  t->trans_tx.start_message = (start_message_t) start_message;
334  t->trans_tx.end_message = (end_message_t) end_message;
335  t->trans_tx.overrun = (overrun_t) overrun;
336  t->trans_tx.count_bytes = (count_bytes_t) count_bytes;
337  t->trans_tx.impl = (void *)(t);
338  PPRZ_MUTEX_INIT(t->mtx_tx); // init mutex, check if correct pointer
339 
340  // add whitelist messages
343 }
344 
345 void gec_dl_init(void)
346 {
347 #ifdef GEC_STATUS_LED
348  LED_OFF(GEC_STATUS_LED);
349 #endif
350 
351  // init pprz transport
352  pprz_transport_init(&gec_tp.pprz_tp);
353 
354  // init crypto transport
356 
357  // initialize keys
359 
360 #if PERIODIC_TELEMETRY
361  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SECURE_LINK_STATUS, send_secure_link_info);
362 #endif
363 }
364 
371 bool gec_encrypt_message(uint8_t *buf, uint8_t *payload_len)
372 {
373  // check payload length
374  if (*payload_len <= PPRZ_MSG_ID) {
375  return false;
376  }
377  // check if the key is ready
378  if (!gec_tp.sts.tx_sym_key.ready) {
379  return false;
380  }
381 
382  // increment counter
384  // convert to bytes
385  uint8_t counter_as_bytes[4];
386  gec_counter_to_bytes(counter, counter_as_bytes);
387  // update nonce
388  memcpy(gec_tp.sts.tx_sym_key.nonce, counter_as_bytes, sizeof(uint32_t));
389 
390  // update intermediate fields
391  uint8_t auth[PPRZ_AUTH_LEN];
392  memcpy(auth, buf, PPRZ_AUTH_LEN);
393  uint8_t tag[PPRZ_MAC_LEN];
394  uint8_t ciphertext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
395  uint32_t mlen = *payload_len - PPRZ_AUTH_LEN; // at least CLASS_BYTE and MSG_ID
396 
397  uint32_t res = Hacl_Chacha20Poly1305_aead_encrypt(ciphertext, tag,
398  &buf[PPRZ_AUTH_LEN], mlen, auth,
399  PPRZ_AUTH_LEN, gec_tp.sts.tx_sym_key.key, gec_tp.sts.tx_sym_key.nonce);
400 
401  if (res == 0) {
402  memset(buf, 0, TRANSPORT_PAYLOAD_LEN); // reset whole buffer
403  *payload_len = 0;
404  buf[PPRZ_GEC_IDX] = PPRZ_MSG_TYPE_ENCRYPTED; // CRYPTO_BYTE
405  *payload_len += 1;
406  memcpy(&buf[PPRZ_CNTR_IDX], counter_as_bytes, sizeof(uint32_t)); // counter
407  *payload_len += sizeof(uint32_t);
408  memcpy(&buf[PPRZ_AUTH_IDX], auth, PPRZ_AUTH_LEN); // auth data
409  *payload_len += PPRZ_AUTH_LEN;
410  memcpy(&buf[PPRZ_CIPH_IDX], ciphertext, mlen); // ciphertext
411  *payload_len += mlen;
412  memcpy(&buf[PPRZ_CIPH_IDX + mlen], tag, PPRZ_MAC_LEN); // tag
413  *payload_len += PPRZ_MAC_LEN;
414  gec_tp.sts.tx_sym_key.counter = counter; // update counter
415  return true;
416  } else {
418  return false;
419  }
420 }
421 
430 bool gec_decrypt_message(uint8_t *buf, volatile uint8_t *payload_len)
431 {
432  switch (buf[PPRZ_GEC_IDX]) {
434  // handle plaintext message
435  if (*payload_len < PPRZ_PLAINTEXT_MSG_MIN_LEN) {
436  return false;
437  }
438  // check if the message is in the whitelist
440  buf[PPRZ_PLAINTEXT_MSG_ID_IDX])) {
441  // return the buffer minus the crypto byte
442  memmove(buf, &buf[1], *payload_len - 1);
443  return true;
444  }
445  return false;
446  break;
448  // attempt decryption
449  if (*payload_len < PPRZ_ENCRYPTED_MSG_MIN_LEN) {
450  return false;
451  }
452  if (!gec_tp.sts.rx_sym_key.ready) {
453  // the rx key is not ready yet
454  return false;
455  }
456  // first check the message counter
458  // check against the saved counter
461  return false;
462  }
463  // update nonce with 4 counter bytes
464  memcpy(gec_tp.sts.rx_sym_key.nonce, &buf[PPRZ_CNTR_IDX],
465  sizeof(uint32_t));
466  // update intermediate fields
467  uint8_t auth[PPRZ_AUTH_LEN];
468  memcpy(auth, &buf[PPRZ_AUTH_IDX], PPRZ_AUTH_LEN);
469  uint8_t tag[PPRZ_MAC_LEN];
470  memcpy(tag, &buf[*payload_len - PPRZ_MAC_LEN], PPRZ_MAC_LEN);
471  uint8_t ciphertext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
472  // payload - AUTH_DATA(2) - TAG(16) - CRYPTO_BYTE(1) - COUNTER(4) = payload_len - 23
473  uint32_t mlen = *payload_len - PPRZ_CRYPTO_OVERHEAD - 1 - PPRZ_AUTH_LEN; // don't forget to count for CRYPTO_BYTE
474  memcpy(ciphertext, &buf[PPRZ_CIPH_IDX], mlen);
475  uint8_t plaintext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
476 
477  // try decryption
478  uint32_t res = Hacl_Chacha20Poly1305_aead_decrypt(plaintext, ciphertext,
479  mlen, tag, auth,
480  PPRZ_AUTH_LEN, gec_tp.sts.rx_sym_key.key,
482 
483  // res == 0 means all good
484  if (res == 0) {
485  memset(buf, 0, TRANSPORT_PAYLOAD_LEN); // reset whole buffer
486  *payload_len = 0;
487  memcpy(buf, auth, PPRZ_AUTH_LEN); // copy auth data
488  *payload_len += PPRZ_AUTH_LEN;
489  memcpy(&buf[PPRZ_AUTH_LEN], plaintext, mlen); // copy plaintext
490  *payload_len += mlen;
491  gec_tp.sts.rx_sym_key.counter = counter; // update counter
492  return true;
493  } else {
495  return false;
496  }
497  break;
498  default:
499  break;
500  }
501  return false;
502 }
503 
533 void gec_dl_event(void)
534 {
535  // strip PPRZ_STX, MSG_LEN, and CHEKSUM, return (CRYPTO_BYTE..MSG_PAYLOAD)
536  pprz_check_and_parse(&DOWNLINK_DEVICE.device, &gec_tp.pprz_tp,
537  gec_tp.pprz_tp.trans_rx.payload, (bool *) &gec_tp.trans_rx.msg_received);
538  // we have (CRYPTO_BYTE .. MSG_PAYLOAD) in
539  if (gec_tp.trans_rx.msg_received) { // self.rx.parse_byte(b)
540  switch (gec_tp.sts.protocol_stage) {
541  case CRYPTO_OK:
542  // decrypt message
543  // if successfull return the message (sender_ID .. MSG_payload)
544  if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
545  &gec_tp.pprz_tp.trans_rx.payload_len)) {
546  // copy the buffer over
547  // NOTE:the real payload_len is at least one byte shorter
548  // but we can copy whole buffer anyway since we don't overflow
549  DatalinkFillDlBuffer(gec_tp.pprz_tp.trans_rx.payload,
550  gec_tp.pprz_tp.trans_rx.payload_len);
551  // pass to datalink
554  }
555  break;
556  case WAIT_MSG1:
557  // decrypt message (checks against the whitelist and removes the crypto byte)
558  // returns (SENDER_ID .. MSG_PAYLOAD
559  if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
560  &gec_tp.pprz_tp.trans_rx.payload_len)) {
561  gec_process_msg1(gec_tp.pprz_tp.trans_rx.payload);
562  } else {
563  }
564  break;
565  case WAIT_MSG3:
566  // decrypt message (checks against the whitelist and removes the crypto byte)
567  // returns (SENDER_ID .. MSG_PAYLOAD
568  if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
569  &gec_tp.pprz_tp.trans_rx.payload_len)) {
570  if (gec_process_msg3(gec_tp.pprz_tp.trans_rx.payload)) {
571  // if OK, we are ready for an ongoing communication
573 #ifdef GEC_STATUS_LED
574  LED_ON(GEC_STATUS_LED);
575 #endif
576  }
577  }
578  break;
579  default:
580  // shouldn't be here unless something is really wrong
581  break;
582  }
583  // reset flag
584  gec_tp.trans_rx.msg_received = false;
585  }
586 }
587 
598 {
599  if (SenderIdOfPprzMsg(buf) != 0) {
600  // process only messages from GCS
601  // log an error
602  return;
603  }
604 
605  // check message ID
606  if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
608  return;
609  }
610 
611  // check message type
612  if (DL_KEY_EXCHANGE_GCS_msg_type(buf) != P_AE) {
614  return;
615  }
616 
617  // check data length
618  if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf) != PPRZ_KEY_LEN) {
620  return;
621  }
622 
623  // copy P_ae over
625  DL_KEY_EXCHANGE_GCS_msg_data(buf), sizeof(struct gec_pubkey));
627 
628  // 2. B generates ephemeral curve25519 key pair (Pbe, Qbe).
631  }
632 
633  // 3. B computes the shared secret: z = scalar_multiplication(Qbe, Pae)
634  uint8_t z[32] = { 0 };
635  Hacl_Curve25519_crypto_scalarmult(z, gec_tp.sts.my_private_ephemeral.priv,
637 
638  // 4. B uses the key derivation function kdf(z,1) to compute Kb || Sb,
639  // kdf(z,0) to compute Ka || Sa, and kdf(z,2) to compute Kclient || Sclient.
641 
642  // 5. B computes the ed25519 signature: sig = signQb(Pbe || Pae)
643  uint8_t sig[PPRZ_SIGN_LEN] = { 0 };
644  uint8_t pbe_concat_p_ae[PPRZ_KEY_LEN * 2] = { 0 };
645  memcpy(pbe_concat_p_ae, &gec_tp.sts.my_private_ephemeral.pub, PPRZ_KEY_LEN);
646  memcpy(&pbe_concat_p_ae[PPRZ_KEY_LEN], &gec_tp.sts.their_public_ephemeral.pub,
647  PPRZ_KEY_LEN);
648  Hacl_Ed25519_sign(sig, gec_tp.sts.my_private_key.priv, pbe_concat_p_ae,
649  PPRZ_KEY_LEN * 2);
650 
651  // 6. B computes and sends the message Pbe || Ekey=Kb,IV=Sb||zero(sig)
652  uint8_t msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN + PPRZ_MAC_LEN] = { 0 };
653  memcpy(msg_data, &gec_tp.sts.my_private_ephemeral.pub, PPRZ_KEY_LEN);
654 
655  if (Hacl_Chacha20Poly1305_aead_encrypt(&msg_data[PPRZ_KEY_LEN],
656  &msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN], sig, PPRZ_SIGN_LEN, NULL, 0,
659  return;
660  }
661 
662 #if PPRZLINK_DEFAULT_VER == 2
663  // now we have to manually construct the message
664  // CRYPTO BYTE
665  // source_id
666  // sender_id
667  // class id
668  // msg id
669  // msg_type
670  // msg_data
672  gec_tp.tx_msg_idx = 1;
673  gec_tp.tx_msg[gec_tp.tx_msg_idx] = AC_ID;
674  gec_tp.tx_msg_idx++;
676  gec_tp.tx_msg_idx++;
677  gec_tp.tx_msg[gec_tp.tx_msg_idx] = 1;// telemetry
678  gec_tp.tx_msg_idx++;
680  gec_tp.tx_msg_idx++;
682  gec_tp.tx_msg_idx++;
683  gec_tp.tx_msg[gec_tp.tx_msg_idx] = sizeof(msg_data);
684  gec_tp.tx_msg_idx++;
685  memcpy(&gec_tp.tx_msg[gec_tp.tx_msg_idx], msg_data, sizeof(msg_data));
686  gec_tp.tx_msg_idx += sizeof(msg_data);
687 
688  struct pprzlink_msg msg2;
689  msg2.trans = &gec_tp.pprz_tp.trans_tx;
690  msg2.dev = &DOWNLINK_DEVICE.device;
691 
692  gec_encapsulate_and_send_msg(&msg2, 0);
693 #else
694 #if PPRZLINK_DEFAULT_VER ==1
695  // Pprzlink 1.0
696  // now we have to manually construct the message
697  // CRYPTO BYTE
698  // source_id
699  // msg id
700  // msg_type
701  // msg_data
703  gec_tp.tx_msg_idx = 1;
704  gec_tp.tx_msg[gec_tp.tx_msg_idx] = AC_ID;
705  gec_tp.tx_msg_idx++;
707  gec_tp.tx_msg_idx++;
709  gec_tp.tx_msg_idx++;
710  gec_tp.tx_msg[gec_tp.tx_msg_idx] = sizeof(msg_data);
711  gec_tp.tx_msg_idx++;
712  memcpy(&gec_tp.tx_msg[gec_tp.tx_msg_idx], msg_data, sizeof(msg_data));
713  gec_tp.tx_msg_idx += sizeof(msg_data);
714 
715  gec_encapsulate_and_send_msg(&gec_tp, &DOWNLINK_DEVICE.device, 0);
716 #endif // PPRZLINK 1.0
717 #endif // PPRZLINK 2.0
718 
719  /*
720  * Note that ideally we would use the following function call, but the problem is that
721  * 'end_message()` cannot currently distinguish between encrypted and plaintext message to be sent
722  * (i.e. I don't know the message ID). The second problem I am using the same buffer here as for
723  * constructing a message, so a corruption of the buffer is possible.
724  *
725  * Since I am doing this only once, the method above is fine for the time being.
726  *
727  * Pprzlink 2.0:
728  uint8_t msg_status = P_BE;
729  pprz_msg_send_KEY_EXCHANGE_UAV(&gec_tp.pprz_tp.trans_tx, &DOWNLINK_DEVICE.device, AC_ID,
730  &msg_status, sizeof(msg_data), msg_data);
731  */
732 
733  // increment status
735 }
736 
745 {
746  if (SenderIdOfPprzMsg(buf) != 0) {
747  // process only messages from GCS
748  // log an error
749  return false;
750  }
751 
752 // check message ID
753  if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
755  return false;
756  }
757 
758 // check message type
759  if (DL_KEY_EXCHANGE_GCS_msg_type(buf) != SIG) {
761  return false;
762  }
763 
764 // check data length
765  if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf)
766  != (PPRZ_SIGN_LEN + PPRZ_MAC_LEN)) {
768  return false;
769  }
770 
771  // 13. B decrypts the message and verifies the signature.
772  uint8_t sign[PPRZ_SIGN_LEN] = { 0 };
773  if (Hacl_Chacha20Poly1305_aead_decrypt(sign,
774  DL_KEY_EXCHANGE_GCS_msg_data(buf),
775  PPRZ_SIGN_LEN, DL_KEY_EXCHANGE_GCS_msg_data(buf) + PPRZ_SIGN_LEN, NULL, 0,
778  return false;
779  }
780 
781  // verify
782  uint8_t p_ae_concat_p_be[PPRZ_KEY_LEN * 2] = { 0 };
783  memcpy(p_ae_concat_p_be, &gec_tp.sts.their_public_ephemeral.pub,
784  PPRZ_KEY_LEN);
785  memcpy(&p_ae_concat_p_be[PPRZ_KEY_LEN], &gec_tp.sts.my_private_ephemeral.pub,
786  PPRZ_KEY_LEN);
787  // returns true if verified properly
788  if (!Hacl_Ed25519_verify(gec_tp.sts.their_public_key.pub, p_ae_concat_p_be,
789  PPRZ_SIGN_LEN, sign)) {
790  // log error and return
792  return false;
793  }
794 
795  // all ok
796  return true;
797 }
798 
799 // add a message id to the whitelist
800 void gec_add_to_whitelist(struct gec_whitelist *whitelist, uint8_t id)
801 {
802  if (whitelist->init && (whitelist->idx < WHITELIST_LEN)) {
803  whitelist->whitelist[whitelist->idx] = id;
804  whitelist->idx++;
805 
806  } else {
807  memset(whitelist, 0, WHITELIST_LEN); // erase the whitelist
808  whitelist->init = true;
809  whitelist->idx = 0;
810  }
811 }
812 
813 // return true if given message id is in the whitelist
814 bool gec_is_in_the_whitelist(struct gec_whitelist *whitelist, uint8_t id)
815 {
816  if (whitelist->init) {
817  for (uint8_t i = 0; i < whitelist->idx; i++) {
818  if (whitelist->whitelist[i] == id) {
819  return true;
820  }
821  }
822  }
823  return false;
824 }
struct sts3032 sts
#define LED_ON(i)
Definition: led_hw.h:51
#define LED_OFF(i)
Definition: led_hw.h:52
void gec_counter_to_bytes(uint32_t n, uint8_t *bytes)
Convert counter to bytes in network byte order.
Definition: gec.c:142
void gec_sts_init(struct gec_sts_ctx *sts)
Definition: gec.c:33
void gec_generate_ephemeral_keys(struct gec_privkey *sk)
Generate private and public key pairs for future use.
Definition: gec.c:73
void gec_derive_key_material(struct gec_sts_ctx *ctx, uint8_t *z)
Derive key material for both sender and receiver.
Definition: gec.c:91
uint32_t gec_bytes_to_counter(uint8_t *bytes)
Convert from network byte order (big endian) to the machine byte order.
Definition: gec.c:117
Galois embedded crypto implementation.
struct gec_pubkey their_public_key
Definition: gec.h:116
uint8_t pub[PPRZ_KEY_LEN]
Definition: gec.h:73
@ WAIT_MSG3
Definition: gec.h:83
@ CRYPTO_OK
Definition: gec.h:83
@ WAIT_MSG1
Definition: gec.h:83
#define PPRZ_KEY_LEN
Definition: gec.h:53
#define PPRZ_MAC_LEN
Definition: gec.h:55
uint8_t pub[PPRZ_KEY_LEN]
Definition: gec.h:69
struct gec_privkey my_private_ephemeral
Definition: gec.h:119
bool ready
Definition: gec.h:73
#define PPRZ_SIGN_LEN
Definition: gec.h:49
struct gec_sym_key tx_sym_key
Definition: gec.h:121
uint8_t nonce[PPRZ_NONCE_LEN]
Definition: gec.h:78
#define PPRZ_CRYPTO_OVERHEAD
Definition: gec.h:61
uint32_t encrypt_err
Definition: gec.h:126
uint8_t priv[PPRZ_KEY_LEN]
Definition: gec.h:68
@ P_BE
Definition: gec.h:91
@ P_AE
Definition: gec.h:91
@ SIG
Definition: gec.h:91
uint32_t rx_counter_err
Definition: gec.h:125
struct gec_pubkey their_public_ephemeral
Definition: gec.h:118
stage_t protocol_stage
Definition: gec.h:122
uint32_t counter
Definition: gec.h:79
@ UNEXPECTED_MSG_ERROR
Definition: gec.h:110
@ MSG3_SIGNVERIFY_ERROR
Definition: gec.h:101
@ MSG1_ENCRYPT_ERROR
Definition: gec.h:98
@ MSG3_DECRYPT_ERROR
Definition: gec.h:100
@ UNEXPECTED_MSG_TYPE_ERROR
Definition: gec.h:108
@ UNEXPECTED_MSG_DATA_ERROR
Definition: gec.h:109
uint32_t decrypt_err
Definition: gec.h:127
#define PPRZ_CNTR_IDX
Definition: gec.h:44
#define PPRZ_MSG_TYPE_ENCRYPTED
Definition: gec.h:40
sts_error_t last_error
Definition: gec.h:124
#define PPRZ_MSG_TYPE_PLAINTEXT
Definition: gec.h:39
#define PPRZ_AUTH_IDX
Definition: gec.h:46
bool ready
Definition: gec.h:69
bool ready
Definition: gec.h:79
struct gec_sym_key rx_sym_key
Definition: gec.h:120
uint8_t key[PPRZ_KEY_LEN]
Definition: gec.h:77
struct gec_privkey my_private_key
Definition: gec.h:117
#define PPRZ_GEC_IDX
Definition: gec.h:42
Definition: gec.h:72
void gec_dl_init(void)
Init function.
Definition: gec_dl.c:345
static void send_secure_link_info(struct transport_tx *trans, struct link_device *dev)
Definition: gec_dl.c:75
bool gec_process_msg3(uint8_t *buf)
Process incoming message (expected MSG3) if the right (KEY_EXCHANGE) message received with the right ...
Definition: gec_dl.c:744
void gec_add_to_whitelist(struct gec_whitelist *whitelist, uint8_t id)
Definition: gec_dl.c:800
#define GEC_UPDATE_DL
Definition: gec_dl.c:67
void gec_dl_event(void)
Parse incoming message bytes (PPRZ_STX..CHCKSUM B) and returns a new decrypted message if it is avail...
Definition: gec_dl.c:533
bool gec_encrypt_message(uint8_t *buf, uint8_t *payload_len)
Attempts message encryption Adds crypto_byte, counter and tag Returns encrypted pprzlink message (cry...
Definition: gec_dl.c:371
struct gec_transport gec_tp
PPRZ transport structure.
Definition: gec_dl.c:70
bool gec_is_in_the_whitelist(struct gec_whitelist *whitelist, uint8_t id)
Definition: gec_dl.c:814
bool gec_decrypt_message(uint8_t *buf, volatile uint8_t *payload_len)
Attemp message decryption If a message is unencrypted, pass it through only if the MSG_ID is in the w...
Definition: gec_dl.c:430
void gec_process_msg1(uint8_t *buf)
NOTE: for RESPONDER party only Process incoming message (expected MSG1) if the right (KEY_EXCHANGE) m...
Definition: gec_dl.c:597
static void insert_byte(struct gec_transport *t, const uint8_t byte)
Simply insert byte to the message buffer.
Definition: gec_dl.c:89
void gec_transport_init(struct gec_transport *t)
Definition: gec_dl.c:325
Datalink using Galois Embedded Crypto.
#define KEY_EXCHANGE_MSG_ID_GCS
Definition: gec_dl.h:45
uint8_t whitelist[WHITELIST_LEN]
Definition: gec_dl.h:54
#define KEY_EXCHANGE_MSG_ID_UAV
Definition: gec_dl.h:44
uint8_t idx
Definition: gec_dl.h:55
bool init
Definition: gec_dl.h:56
#define WHITELIST_LEN
Definition: gec_dl.h:46
Whitelist for sending and receiving unencrypted messages (mostly for KEY_EXCHANGE messages)
Definition: gec_dl.h:53
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
uint32_t counter
Definition: ins_flow.c:192
arch independent LED (Light Emitting Diodes) API
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:63
bool dl_msg_available
Flag provided to control calls to dl_parse_msg.
Definition: main_demo5.c:60
static float sign(float x)
sign function
Definition: nav_fish.c:232
#define byte
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
#define PPRZ_MUTEX_INIT(_mtx)
Definition: pprz_mutex.h:45
int fd
Definition: serial.c:26
#define DOWNLINK_DEVICE
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
uint8_t tx_msg_idx
Definition: gec_dl.h:80
struct gec_sts_ctx sts
Definition: gec_dl.h:83
uint8_t tx_msg[TRANSPORT_PAYLOAD_LEN]
Definition: gec_dl.h:79
struct transport_tx trans_tx
Definition: gec_dl.h:77
struct transport_rx trans_rx
Definition: gec_dl.h:74
struct pprz_transport pprz_tp
Definition: gec_dl.h:71
struct gec_whitelist whitelist
Definition: gec_dl.h:84
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float b
Definition: wedgebug.c:202