Paparazzi UAS  v5.15_devel-109-gee85905
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
67 
68 #if PERIODIC_TELEMETRY
70 
71 static void send_secure_link_info(struct transport_tx *trans, struct link_device *dev)
72 {
73  pprz_msg_send_SECURE_LINK_STATUS(trans, dev, AC_ID,
79 }
80 #endif
81 
85 static inline void insert_byte(struct gec_transport *t, const uint8_t byte)
86 {
87  t->tx_msg[t->tx_msg_idx] = byte;
88  t->tx_msg_idx++;
89 }
90 
91 #if PPRZLINK_DEFAULT_VER == 2
92 void gec_encapsulate_and_send_msg(struct pprzlink_msg *msg, long fd);
93 
97 static struct gec_transport *get_trans(struct pprzlink_msg *msg)
98 {
99  return (struct gec_transport *)(msg->trans->impl);
100 }
101 
105 static void put_bytes(struct pprzlink_msg *msg, long fd __attribute__((unused)),
106  enum TransportDataType type __attribute__((unused)),
107  enum TransportDataFormat format __attribute__((unused)), const void *bytes,
108  uint16_t len)
109 {
110  const uint8_t *b = (const uint8_t *) bytes;
111  int i;
112  for (i = 0; i < len; i++) {
113  insert_byte(get_trans(msg), b[i]);
114  }
115 }
116 
117 static void put_named_byte(struct pprzlink_msg *msg,
118  long fd __attribute__((unused)),
119  enum TransportDataType type __attribute__((unused)),
120  enum TransportDataFormat format __attribute__((unused)), uint8_t byte,
121  const char *name __attribute__((unused)))
122 {
123  insert_byte(get_trans(msg), byte);
124 }
125 
130 static uint8_t size_of(struct pprzlink_msg *msg, uint8_t len)
131 {
132  return PPRZ_CRYPTO_OVERHEAD + 1
133  + get_trans(msg)->pprz_tp.trans_tx.size_of(msg, len);
134 }
135 
139 static void start_message(struct pprzlink_msg *msg,
140  long fd __attribute__((unused)),
141  uint8_t payload_len __attribute__((unused)))
142 {
143  PPRZ_MUTEX_LOCK(get_trans(msg)->mtx_tx); // lock mutex
144  memset(get_trans(msg)->tx_msg, _FD, TRANSPORT_PAYLOAD_LEN);// erase message data
145  get_trans(msg)->tx_msg_idx = 0;// reset index
146 }
147 
151 static void overrun(struct pprzlink_msg *msg)
152 {
153  get_trans(msg)->pprz_tp.trans_tx.overrun(msg);
154 }
155 
159 static void count_bytes(struct pprzlink_msg *msg, uint8_t bytes)
160 {
161  get_trans(msg)->pprz_tp.trans_tx.count_bytes(msg, bytes);
162 }
163 
173 static int check_available_space(struct pprzlink_msg *msg, long *fd,
174  uint16_t bytes)
175 {
176  if (get_trans(msg)->sts.protocol_stage == CRYPTO_OK) {
177  return get_trans(msg)->pprz_tp.trans_tx.check_available_space(msg, fd,
178  bytes);
179  }
180  return 0;
181 }
182 
190 static void end_message(struct pprzlink_msg *msg, long fd)
191 {
192  switch (gec_tp.sts.protocol_stage) {
193  case CRYPTO_OK:
195  gec_encapsulate_and_send_msg(msg, fd);
196  }
197  break;
198  default:
199  // shouldn't be here as sending messages is not allowed until after the key exchange
200  break;
201  }
202  // unlock mutex
203  PPRZ_MUTEX_UNLOCK(get_trans(msg)->mtx_tx);
204 }
205 
211 void gec_encapsulate_and_send_msg(struct pprzlink_msg *msg, long fd)
212 {
213  get_trans(msg)->pprz_tp.trans_tx.start_message(msg, fd,
214  get_trans(msg)->tx_msg_idx);
215  get_trans(msg)->pprz_tp.trans_tx.put_bytes(msg, _FD, DL_TYPE_UINT8,
216  DL_FORMAT_SCALAR, get_trans(msg)->tx_msg, get_trans(msg)->tx_msg_idx);
217  get_trans(msg)->pprz_tp.trans_tx.end_message(msg, fd);
218 }
219 
220 #else
221 #if PPRZLINK_DEFAULT_VER == 1
222 void gec_encapsulate_and_send_msg(struct gec_transport *trans,
223  struct link_device *dev, long fd);
224 
225 static void put_bytes(struct gec_transport *trans,
226  struct link_device *dev __attribute__((unused)),
227  long fd __attribute__((unused)),
228  enum TransportDataType type __attribute__((unused)),
229  enum TransportDataFormat format __attribute__((unused)), const void *bytes,
230  uint16_t len)
231 {
232  const uint8_t *b = (const uint8_t *) bytes;
233  int i;
234  for (i = 0; i < len; i++) {
235  insert_byte(trans, b[i]);
236  }
237 }
238 
239 static void put_named_byte(struct gec_transport *trans,
240  struct link_device *dev __attribute__((unused)),
241  long fd __attribute__((unused)),
242  enum TransportDataType type __attribute__((unused)),
243  enum TransportDataFormat format __attribute__((unused)), uint8_t byte,
244  const char *name __attribute__((unused)))
245 {
246  insert_byte(trans, byte);
247 }
248 
249 static uint8_t size_of(struct gec_transport *trans __attribute__((unused)),
250  uint8_t len)
251 {
252  return PPRZ_CRYPTO_OVERHEAD + 1 + trans->pprz_tp.trans_tx.size_of(trans, len);
253 }
254 
255 static void start_message(struct gec_transport *trans,
256  struct link_device *dev __attribute__((unused)),
257  long fd __attribute__((unused)),
258  uint8_t payload_len __attribute__((unused)))
259 {
260  PPRZ_MUTEX_LOCK(trans->mtx_tx); // lock mutex
261  memset(trans->tx_msg, _FD, TRANSPORT_PAYLOAD_LEN); // erase message data
262  trans->tx_msg_idx = 0; // reset index
263 }
264 
265 static void end_message(struct gec_transport *trans, struct link_device *dev,
266  long fd)
267 {
268  switch (gec_tp.sts.protocol_stage) {
269  case CRYPTO_OK:
271  gec_encapsulate_and_send_msg(trans, dev, fd);
272  }
273  break;
274  default:
275  // shouldn't be here as sending messages is not allowed until after the key exchange
276  break;
277  }
278  // unlock mutex
279  PPRZ_MUTEX_UNLOCK(trans->mtx_tx);
280 }
281 
282 void gec_encapsulate_and_send_msg(struct gec_transport *trans,
283  struct link_device *dev, long fd)
284 {
285  trans->pprz_tp.trans_tx.start_message(trans, dev, fd, trans->tx_msg_idx);
286  trans->pprz_tp.trans_tx.put_bytes(trans, dev, _FD, DL_TYPE_UINT8,
287  DL_FORMAT_SCALAR, trans->tx_msg, trans->tx_msg_idx);
288  trans->pprz_tp.trans_tx.end_message(trans, dev, fd);
289 }
290 
291 static void overrun(struct gec_transport *trans __attribute__((unused)),
292  struct link_device *dev)
293 {
294  trans->pprz_tp.trans_tx.overrun(trans, dev);
295 }
296 
297 static void count_bytes(struct gec_transport *trans __attribute__((unused)),
298  struct link_device *dev, uint8_t bytes)
299 {
300  trans->pprz_tp.trans_tx.count_bytes(trans, dev, bytes);
301 }
302 
303 static int check_available_space(
304  struct gec_transport *trans __attribute__((unused)),
305  struct link_device *dev, long *fd, uint16_t bytes)
306 {
307  if (trans->sts.protocol_stage == CRYPTO_OK) {
308  return trans->pprz_tp.trans_tx.check_available_space(trans, dev, fd, bytes);
309  }
310  return 0;
311 }
312 #endif // PPRZLINK_DEFAULT_VER == 1
313 #endif // PPRZLINK_DEFAULT_VER == 2
314 
315 // Init pprz transport structure
317 {
318  t->trans_rx.msg_received = false;
319  t->trans_tx.size_of = (size_of_t) size_of;
320  t->trans_tx.check_available_space =
321  (check_available_space_t) check_available_space;
322  t->trans_tx.put_bytes = (put_bytes_t) put_bytes;
323  t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte;
324  t->trans_tx.start_message = (start_message_t) start_message;
325  t->trans_tx.end_message = (end_message_t) end_message;
326  t->trans_tx.overrun = (overrun_t) overrun;
327  t->trans_tx.count_bytes = (count_bytes_t) count_bytes;
328  t->trans_tx.impl = (void *)(t);
329  PPRZ_MUTEX_INIT(t->mtx_tx); // init mutex, check if correct pointer
330 
331  // add whitelist messages
334 }
335 
336 void gec_dl_init(void)
337 {
338 #ifdef GEC_STATUS_LED
339  LED_OFF(GEC_STATUS_LED);
340 #endif
341 
342  // init pprz transport
343  pprz_transport_init(&gec_tp.pprz_tp);
344 
345  // init crypto transport
347 
348  // initialize keys
350 
351 #if PERIODIC_TELEMETRY
352  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SECURE_LINK_STATUS, send_secure_link_info);
353 #endif
354 }
355 
362 bool gec_encrypt_message(uint8_t *buf, uint8_t *payload_len)
363 {
364  // check payload length
365  if (*payload_len <= PPRZ_MSG_ID) {
366  return false;
367  }
368  // check if the key is ready
369  if (!gec_tp.sts.tx_sym_key.ready) {
370  return false;
371  }
372 
373  // increment counter
375  // convert to bytes
376  uint8_t counter_as_bytes[4];
377  gec_counter_to_bytes(counter, counter_as_bytes);
378  // update nonce
379  memcpy(gec_tp.sts.tx_sym_key.nonce, counter_as_bytes, sizeof(uint32_t));
380 
381  // update intermediate fields
382  uint8_t auth[PPRZ_AUTH_LEN];
383  memcpy(auth, buf, PPRZ_AUTH_LEN);
384  uint8_t tag[PPRZ_MAC_LEN];
385  uint8_t ciphertext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
386  uint32_t mlen = *payload_len - PPRZ_AUTH_LEN; // at least CLASS_BYTE and MSG_ID
387 
388  uint32_t res = Hacl_Chacha20Poly1305_aead_encrypt(ciphertext, tag,
389  &buf[PPRZ_AUTH_LEN], mlen, auth,
390  PPRZ_AUTH_LEN, gec_tp.sts.tx_sym_key.key, gec_tp.sts.tx_sym_key.nonce);
391 
392  if (res == 0) {
393  memset(buf, 0, TRANSPORT_PAYLOAD_LEN); // reset whole buffer
394  *payload_len = 0;
395  buf[PPRZ_GEC_IDX] = PPRZ_MSG_TYPE_ENCRYPTED; // CRYPTO_BYTE
396  *payload_len += 1;
397  memcpy(&buf[PPRZ_CNTR_IDX], counter_as_bytes, sizeof(uint32_t)); // counter
398  *payload_len += sizeof(uint32_t);
399  memcpy(&buf[PPRZ_AUTH_IDX], auth, PPRZ_AUTH_LEN); // auth data
400  *payload_len += PPRZ_AUTH_LEN;
401  memcpy(&buf[PPRZ_CIPH_IDX], ciphertext, mlen); // ciphertext
402  *payload_len += mlen;
403  memcpy(&buf[PPRZ_CIPH_IDX + mlen], tag, PPRZ_MAC_LEN); // tag
404  *payload_len += PPRZ_MAC_LEN;
405  gec_tp.sts.tx_sym_key.counter = counter; // update counter
406  return true;
407  } else {
409  return false;
410  }
411 }
412 
421 bool gec_decrypt_message(uint8_t *buf, volatile uint8_t *payload_len)
422 {
423  switch (buf[PPRZ_GEC_IDX]) {
425  // handle plaintext message
426  if (*payload_len < PPRZ_PLAINTEXT_MSG_MIN_LEN) {
427  return false;
428  }
429  // check if the message is in the whitelist
431  buf[PPRZ_PLAINTEXT_MSG_ID_IDX])) {
432  // return the buffer minus the crypto byte
433  memmove(buf, &buf[1], *payload_len - 1);
434  return true;
435  }
436  return false;
437  break;
439  // attempt decryption
440  if (*payload_len < PPRZ_ENCRYPTED_MSG_MIN_LEN) {
441  return false;
442  }
443  if (!gec_tp.sts.rx_sym_key.ready) {
444  // the rx key is not ready yet
445  return false;
446  }
447  // first check the message counter
449  // check against the saved counter
450  if (counter <= gec_tp.sts.rx_sym_key.counter) {
452  return false;
453  }
454  // update nonce with 4 counter bytes
455  memcpy(gec_tp.sts.rx_sym_key.nonce, &buf[PPRZ_CNTR_IDX],
456  sizeof(uint32_t));
457  // update intermediate fields
458  uint8_t auth[PPRZ_AUTH_LEN];
459  memcpy(auth, &buf[PPRZ_AUTH_IDX], PPRZ_AUTH_LEN);
460  uint8_t tag[PPRZ_MAC_LEN];
461  memcpy(tag, &buf[*payload_len - PPRZ_MAC_LEN], PPRZ_MAC_LEN);
462  uint8_t ciphertext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
463  // payload - AUTH_DATA(2) - TAG(16) - CRYPTO_BYTE(1) - COUNTER(4) = payload_len - 23
464  uint32_t mlen = *payload_len - PPRZ_CRYPTO_OVERHEAD - 1 - PPRZ_AUTH_LEN; // don't forget to count for CRYPTO_BYTE
465  memcpy(ciphertext, &buf[PPRZ_CIPH_IDX], mlen);
466  uint8_t plaintext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
467 
468  // try decryption
469  uint32_t res = Hacl_Chacha20Poly1305_aead_decrypt(plaintext, ciphertext,
470  mlen, tag, auth,
471  PPRZ_AUTH_LEN, gec_tp.sts.rx_sym_key.key,
473 
474  // res == 0 means all good
475  if (res == 0) {
476  memset(buf, 0, TRANSPORT_PAYLOAD_LEN); // reset whole buffer
477  *payload_len = 0;
478  memcpy(buf, auth, PPRZ_AUTH_LEN); // copy auth data
479  *payload_len += PPRZ_AUTH_LEN;
480  memcpy(&buf[PPRZ_AUTH_LEN], plaintext, mlen); // copy plaintext
481  *payload_len += mlen;
482  gec_tp.sts.rx_sym_key.counter = counter; // update counter
483  return true;
484  } else {
486  return false;
487  }
488  break;
489  default:
490  break;
491  }
492  return false;
493 }
494 
524 void gec_dl_event(void)
525 {
526  // strip PPRZ_STX, MSG_LEN, and CHEKSUM, return (CRYPTO_BYTE..MSG_PAYLOAD)
527  pprz_check_and_parse(&DOWNLINK_DEVICE.device, &gec_tp.pprz_tp,
528  gec_tp.pprz_tp.trans_rx.payload, (bool *) &gec_tp.trans_rx.msg_received);
529  // we have (CRYPTO_BYTE .. MSG_PAYLOAD) in
530  if (gec_tp.trans_rx.msg_received) { // self.rx.parse_byte(b)
531  switch (gec_tp.sts.protocol_stage) {
532  case CRYPTO_OK:
533  // decrypt message
534  // if successfull return the message (sender_ID .. MSG_payload)
535  if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
536  &gec_tp.pprz_tp.trans_rx.payload_len)) {
537  // copy the buffer over
538  // NOTE:the real payload_len is at least one byte shorter
539  // but we can copy whole buffer anyway since we don't overflow
540  DatalinkFillDlBuffer(gec_tp.pprz_tp.trans_rx.payload,
541  gec_tp.pprz_tp.trans_rx.payload_len);
542  // pass to datalink
545  }
546  break;
547  case WAIT_MSG1:
548  // decrypt message (checks against the whitelist and removes the crypto byte)
549  // returns (SENDER_ID .. MSG_PAYLOAD
550  if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
551  &gec_tp.pprz_tp.trans_rx.payload_len)) {
552  gec_process_msg1(gec_tp.pprz_tp.trans_rx.payload);
553  } else {
554  }
555  break;
556  case WAIT_MSG3:
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  if (gec_process_msg3(gec_tp.pprz_tp.trans_rx.payload)) {
562  // if OK, we are ready for an ongoing communication
564 #ifdef GEC_STATUS_LED
565  LED_ON(GEC_STATUS_LED);
566 #endif
567  }
568  }
569  break;
570  default:
571  // shouldn't be here unless something is really wrong
572  break;
573  }
574  // reset flag
575  gec_tp.trans_rx.msg_received = false;
576  }
577 }
578 
589 {
590  if (SenderIdOfPprzMsg(buf) != 0) {
591  // process only messages from GCS
592  // log an error
593  return;
594  }
595 
596  // check message ID
597  if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
599  return;
600  }
601 
602  // check message type
603  if (DL_KEY_EXCHANGE_GCS_msg_type(buf) != P_AE) {
605  return;
606  }
607 
608  // check data length
609  if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf) != PPRZ_KEY_LEN) {
611  return;
612  }
613 
614  // copy P_ae over
616  DL_KEY_EXCHANGE_GCS_msg_data(buf), sizeof(struct gec_pubkey));
618 
619  // 2. B generates ephemeral curve25519 key pair (Pbe, Qbe).
622  }
623 
624  // 3. B computes the shared secret: z = scalar_multiplication(Qbe, Pae)
625  uint8_t z[32] = { 0 };
626  Hacl_Curve25519_crypto_scalarmult(z, gec_tp.sts.my_private_ephemeral.priv,
628 
629  // 4. B uses the key derivation function kdf(z,1) to compute Kb || Sb,
630  // kdf(z,0) to compute Ka || Sa, and kdf(z,2) to compute Kclient || Sclient.
632 
633  // 5. B computes the ed25519 signature: sig = signQb(Pbe || Pae)
634  uint8_t sig[PPRZ_SIGN_LEN] = { 0 };
635  uint8_t pbe_concat_p_ae[PPRZ_KEY_LEN * 2] = { 0 };
636  memcpy(pbe_concat_p_ae, &gec_tp.sts.my_private_ephemeral.pub, PPRZ_KEY_LEN);
637  memcpy(&pbe_concat_p_ae[PPRZ_KEY_LEN], &gec_tp.sts.their_public_ephemeral.pub,
638  PPRZ_KEY_LEN);
639  Hacl_Ed25519_sign(sig, gec_tp.sts.my_private_key.priv, pbe_concat_p_ae,
640  PPRZ_KEY_LEN * 2);
641 
642  // 6. B computes and sends the message Pbe || Ekey=Kb,IV=Sb||zero(sig)
643  uint8_t msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN + PPRZ_MAC_LEN] = { 0 };
644  memcpy(msg_data, &gec_tp.sts.my_private_ephemeral.pub, PPRZ_KEY_LEN);
645 
646  if (Hacl_Chacha20Poly1305_aead_encrypt(&msg_data[PPRZ_KEY_LEN],
647  &msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN], sig, PPRZ_SIGN_LEN, NULL, 0,
650  return;
651  }
652 
653 #if PPRZLINK_DEFAULT_VER == 2
654  // now we have to manually construct the message
655  // CRYPTO BYTE
656  // source_id
657  // sender_id
658  // class id
659  // msg id
660  // msg_type
661  // msg_data
663  gec_tp.tx_msg_idx = 1;
664  gec_tp.tx_msg[gec_tp.tx_msg_idx] = AC_ID;
665  gec_tp.tx_msg_idx++;
667  gec_tp.tx_msg_idx++;
668  gec_tp.tx_msg[gec_tp.tx_msg_idx] = 1;// telemetry
669  gec_tp.tx_msg_idx++;
671  gec_tp.tx_msg_idx++;
673  gec_tp.tx_msg_idx++;
674  gec_tp.tx_msg[gec_tp.tx_msg_idx] = sizeof(msg_data);
675  gec_tp.tx_msg_idx++;
676  memcpy(&gec_tp.tx_msg[gec_tp.tx_msg_idx], msg_data, sizeof(msg_data));
677  gec_tp.tx_msg_idx += sizeof(msg_data);
678 
679  struct pprzlink_msg msg2;
680  msg2.trans = &gec_tp.pprz_tp.trans_tx;
681  msg2.dev = &DOWNLINK_DEVICE.device;
682 
683  gec_encapsulate_and_send_msg(&msg2, 0);
684 #else
685 #if PPRZLINK_DEFAULT_VER ==1
686  // Pprzlink 1.0
687  // now we have to manually construct the message
688  // CRYPTO BYTE
689  // source_id
690  // msg id
691  // msg_type
692  // msg_data
694  gec_tp.tx_msg_idx = 1;
695  gec_tp.tx_msg[gec_tp.tx_msg_idx] = AC_ID;
696  gec_tp.tx_msg_idx++;
698  gec_tp.tx_msg_idx++;
700  gec_tp.tx_msg_idx++;
701  gec_tp.tx_msg[gec_tp.tx_msg_idx] = sizeof(msg_data);
702  gec_tp.tx_msg_idx++;
703  memcpy(&gec_tp.tx_msg[gec_tp.tx_msg_idx], msg_data, sizeof(msg_data));
704  gec_tp.tx_msg_idx += sizeof(msg_data);
705 
706  gec_encapsulate_and_send_msg(&gec_tp, &DOWNLINK_DEVICE.device, 0);
707 #endif // PPRZLINK 1.0
708 #endif // PPRZLINK 2.0
709 
710  /*
711  * Note that ideally we would use the following function call, but the problem is that
712  * 'end_message()` cannot currently distinguish between encrypted and plaintext message to be sent
713  * (i.e. I don't know the message ID). The second problem I am using the same buffer here as for
714  * constructing a message, so a corruption of the buffer is possible.
715  *
716  * Since I am doing this only once, the method above is fine for the time being.
717  *
718  * Pprzlink 2.0:
719  uint8_t msg_status = P_BE;
720  pprz_msg_send_KEY_EXCHANGE_UAV(&gec_tp.pprz_tp.trans_tx, &DOWNLINK_DEVICE.device, AC_ID,
721  &msg_status, sizeof(msg_data), msg_data);
722  */
723 
724  // increment status
726 }
727 
736 {
737  if (SenderIdOfPprzMsg(buf) != 0) {
738  // process only messages from GCS
739  // log an error
740  return false;
741  }
742 
743 // check message ID
744  if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
746  return false;
747  }
748 
749 // check message type
750  if (DL_KEY_EXCHANGE_GCS_msg_type(buf) != SIG) {
752  return false;
753  }
754 
755 // check data length
756  if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf)
757  != (PPRZ_SIGN_LEN + PPRZ_MAC_LEN)) {
759  return false;
760  }
761 
762  // 13. B decrypts the message and verifies the signature.
763  uint8_t sign[PPRZ_SIGN_LEN] = { 0 };
764  if (Hacl_Chacha20Poly1305_aead_decrypt(sign,
765  DL_KEY_EXCHANGE_GCS_msg_data(buf),
766  PPRZ_SIGN_LEN, DL_KEY_EXCHANGE_GCS_msg_data(buf) + PPRZ_SIGN_LEN, NULL, 0,
769  return false;
770  }
771 
772  // verify
773  uint8_t p_ae_concat_p_be[PPRZ_KEY_LEN * 2] = { 0 };
774  memcpy(p_ae_concat_p_be, &gec_tp.sts.their_public_ephemeral.pub,
775  PPRZ_KEY_LEN);
776  memcpy(&p_ae_concat_p_be[PPRZ_KEY_LEN], &gec_tp.sts.my_private_ephemeral.pub,
777  PPRZ_KEY_LEN);
778  // returns true if verified properly
779  if (!Hacl_Ed25519_verify(gec_tp.sts.their_public_key.pub, p_ae_concat_p_be,
780  PPRZ_SIGN_LEN, sign)) {
781  // log error and return
783  return false;
784  }
785 
786  // all ok
787  return true;
788 }
789 
790 // add a message id to the whitelist
791 void gec_add_to_whitelist(struct gec_whitelist *whitelist, uint8_t id)
792 {
793  if (whitelist->init && (whitelist->idx < WHITELIST_LEN)) {
794  whitelist->whitelist[whitelist->idx] = id;
795  whitelist->idx++;
796 
797  } else {
798  memset(whitelist, 0, WHITELIST_LEN); // erase the whitelist
799  whitelist->init = true;
800  whitelist->idx = 0;
801  }
802 }
803 
804 // return true if given message id is in the whitelist
805 bool gec_is_in_the_whitelist(struct gec_whitelist *whitelist, uint8_t id)
806 {
807  if (whitelist->init) {
808  for (uint8_t i = 0; i < whitelist->idx; i++) {
809  if (whitelist->whitelist[i] == id) {
810  return true;
811  }
812  }
813  }
814  return false;
815 }
unsigned short uint16_t
Definition: types.h:16
#define KEY_EXCHANGE_MSG_ID_UAV
Definition: gec_dl.h:44
uint8_t key[PPRZ_KEY_LEN]
Definition: gec.h:77
bool gec_is_in_the_whitelist(struct gec_whitelist *whitelist, uint8_t id)
Definition: gec_dl.c:805
Definition: gec.h:83
#define DOWNLINK_DEVICE
bool ready
Definition: gec.h:79
bool ready
Definition: gec.h:69
#define PPRZ_SIGN_LEN
Definition: gec.h:49
bool init
Definition: gec_dl.h:56
Periodic telemetry system header (includes downlink utility and generated code).
#define LED_OFF(i)
Definition: led_hw.h:52
uint8_t pub[PPRZ_KEY_LEN]
Definition: gec.h:73
struct gec_privkey my_private_ephemeral
Definition: gec.h:119
void gec_dl_init(void)
Init function.
Definition: gec_dl.c:336
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
Definition: gec.h:91
Galois embedded crypto implementation.
uint8_t idx
Definition: gec_dl.h:55
struct gec_sts_ctx sts
Definition: gec_dl.h:83
struct gec_pubkey their_public_key
Definition: gec.h:116
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 encrypt_err
Definition: gec.h:126
stage_t protocol_stage
Definition: gec.h:122
void gec_add_to_whitelist(struct gec_whitelist *whitelist, uint8_t id)
Definition: gec_dl.c:791
bool dl_msg_available
Definition: main_demo5.c:61
static void send_secure_link_info(struct transport_tx *trans, struct link_device *dev)
Definition: gec_dl.c:71
struct transport_tx trans_tx
Definition: gec_dl.h:77
#define PPRZ_CRYPTO_OVERHEAD
Definition: gec.h:61
#define WHITELIST_LEN
Definition: gec_dl.h:46
uint32_t decrypt_err
Definition: gec.h:127
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
struct gec_transport gec_tp
PPRZ transport structure.
Definition: gec_dl.c:66
struct gec_sym_key rx_sym_key
Definition: gec.h:120
uint8_t pub[PPRZ_KEY_LEN]
Definition: gec.h:69
struct gec_pubkey their_public_ephemeral
Definition: gec.h:118
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:362
unsigned long uint32_t
Definition: types.h:18
int32_t counter
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void gec_sts_init(struct gec_sts_ctx *sts)
Definition: gec.c:33
struct gec_whitelist whitelist
Definition: gec_dl.h:84
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:421
struct pprz_transport pprz_tp
Definition: gec_dl.h:71
bool ready
Definition: gec.h:73
uint8_t tx_msg[TRANSPORT_PAYLOAD_LEN]
Definition: gec_dl.h:79
#define PPRZ_MUTEX_INIT(_mtx)
Definition: pprz_mutex.h:45
Definition: gec.h:83
void gec_transport_init(struct gec_transport *t)
Definition: gec_dl.c:316
Whitelist for sending and receiving unencrypted messages (mostly for KEY_EXCHANGE messages) ...
Definition: gec_dl.h:53
sts_error_t last_error
Definition: gec.h:124
uint8_t whitelist[WHITELIST_LEN]
Definition: gec_dl.h:54
#define PPRZ_MAC_LEN
Definition: gec.h:55
#define PPRZ_MSG_TYPE_PLAINTEXT
Definition: gec.h:39
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
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:735
struct gec_sym_key tx_sym_key
Definition: gec.h:121
unsigned char uint8_t
Definition: types.h:14
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:588
uint8_t nonce[PPRZ_NONCE_LEN]
Definition: gec.h:78
#define byte
Definition: gec.h:91
static void insert_byte(struct gec_transport *t, const uint8_t byte)
Simply insert byte to the message buffer.
Definition: gec_dl.c:85
#define PPRZ_MSG_TYPE_ENCRYPTED
Definition: gec.h:40
int fd
Definition: serial.c:26
#define KEY_EXCHANGE_MSG_ID_GCS
Definition: gec_dl.h:45
Definition: gec.h:72
Definition: gec.h:83
#define PPRZ_GEC_IDX
Definition: gec.h:42
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
void gec_generate_ephemeral_keys(struct gec_privkey *sk)
Generate private and public key pairs for future use.
Definition: gec.c:73
uint32_t counter
Definition: gec.h:79
#define PPRZ_CNTR_IDX
Definition: gec.h:44
arch independent LED (Light Emitting Diodes) API
uint32_t rx_counter_err
Definition: gec.h:125
void gec_counter_to_bytes(uint32_t n, uint8_t *bytes)
Convert counter to bytes in network byte order.
Definition: gec.c:142
uint8_t priv[PPRZ_KEY_LEN]
Definition: gec.h:68
#define LED_ON(i)
Definition: led_hw.h:51
struct transport_rx trans_rx
Definition: gec_dl.h:74
#define PPRZ_AUTH_IDX
Definition: gec.h:46
uint8_t tx_msg_idx
Definition: gec_dl.h:80
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
Datalink using Galois Embedded Crypto.
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
#define PPRZ_KEY_LEN
Definition: gec.h:53
struct gec_privkey my_private_key
Definition: gec.h:117
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:524
Definition: gec.h:91