Paparazzi UAS  v5.17_devel-14-g4575375
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 
66 #ifndef GEC_UPDATE_DL
67 #define GEC_UPDATE_DL TRUE
68 #endif
69 
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, _FD, 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  }
201  break;
202  default:
203  // shouldn't be here as sending messages is not allowed until after the key exchange
204  break;
205  }
206  // unlock mutex
207  PPRZ_MUTEX_UNLOCK(get_trans(msg)->mtx_tx);
208 }
209 
215 void gec_encapsulate_and_send_msg(struct pprzlink_msg *msg, long fd)
216 {
217  get_trans(msg)->pprz_tp.trans_tx.start_message(msg, fd,
218  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  get_trans(msg)->pprz_tp.trans_tx.end_message(msg, fd);
222 }
223 
224 #else
225 #if PPRZLINK_DEFAULT_VER == 1
226 void gec_encapsulate_and_send_msg(struct gec_transport *trans,
227  struct link_device *dev, long fd);
228 
229 static void put_bytes(struct gec_transport *trans,
230  struct link_device *dev __attribute__((unused)),
231  long fd __attribute__((unused)),
232  enum TransportDataType type __attribute__((unused)),
233  enum TransportDataFormat format __attribute__((unused)), const void *bytes,
234  uint16_t len)
235 {
236  const uint8_t *b = (const uint8_t *) bytes;
237  int i;
238  for (i = 0; i < len; i++) {
239  insert_byte(trans, b[i]);
240  }
241 }
242 
243 static void put_named_byte(struct gec_transport *trans,
244  struct link_device *dev __attribute__((unused)),
245  long fd __attribute__((unused)),
246  enum TransportDataType type __attribute__((unused)),
247  enum TransportDataFormat format __attribute__((unused)), uint8_t byte,
248  const char *name __attribute__((unused)))
249 {
250  insert_byte(trans, byte);
251 }
252 
253 static uint8_t size_of(struct gec_transport *trans __attribute__((unused)),
254  uint8_t len)
255 {
256  return PPRZ_CRYPTO_OVERHEAD + 1 + trans->pprz_tp.trans_tx.size_of(trans, len);
257 }
258 
259 static void start_message(struct gec_transport *trans,
260  struct link_device *dev __attribute__((unused)),
261  long fd __attribute__((unused)),
262  uint8_t payload_len __attribute__((unused)))
263 {
264  PPRZ_MUTEX_LOCK(trans->mtx_tx); // lock mutex
265  memset(trans->tx_msg, _FD, TRANSPORT_PAYLOAD_LEN); // erase message data
266  trans->tx_msg_idx = 0; // reset index
267 }
268 
269 static void end_message(struct gec_transport *trans, struct link_device *dev,
270  long fd)
271 {
272  switch (gec_tp.sts.protocol_stage) {
273  case CRYPTO_OK:
275  gec_encapsulate_and_send_msg(trans, dev, fd);
276  }
277  break;
278  default:
279  // shouldn't be here as sending messages is not allowed until after the key exchange
280  break;
281  }
282  // unlock mutex
283  PPRZ_MUTEX_UNLOCK(trans->mtx_tx);
284 }
285 
286 void gec_encapsulate_and_send_msg(struct gec_transport *trans,
287  struct link_device *dev, long fd)
288 {
289  trans->pprz_tp.trans_tx.start_message(trans, dev, fd, trans->tx_msg_idx);
290  trans->pprz_tp.trans_tx.put_bytes(trans, dev, _FD, DL_TYPE_UINT8,
291  DL_FORMAT_SCALAR, trans->tx_msg, trans->tx_msg_idx);
292  trans->pprz_tp.trans_tx.end_message(trans, dev, fd);
293 }
294 
295 static void overrun(struct gec_transport *trans __attribute__((unused)),
296  struct link_device *dev)
297 {
298  trans->pprz_tp.trans_tx.overrun(trans, dev);
299 }
300 
301 static void count_bytes(struct gec_transport *trans __attribute__((unused)),
302  struct link_device *dev, uint8_t bytes)
303 {
304  trans->pprz_tp.trans_tx.count_bytes(trans, dev, bytes);
305 }
306 
307 static int check_available_space(
308  struct gec_transport *trans __attribute__((unused)),
309  struct link_device *dev, long *fd, uint16_t bytes)
310 {
311  if (trans->sts.protocol_stage == CRYPTO_OK) {
312  return trans->pprz_tp.trans_tx.check_available_space(trans, dev, fd, bytes);
313  }
314  return 0;
315 }
316 #endif // PPRZLINK_DEFAULT_VER == 1
317 #endif // PPRZLINK_DEFAULT_VER == 2
318 
319 // Init pprz transport structure
321 {
322  t->trans_rx.msg_received = false;
323  t->trans_tx.size_of = (size_of_t) size_of;
324  t->trans_tx.check_available_space =
325  (check_available_space_t) check_available_space;
326  t->trans_tx.put_bytes = (put_bytes_t) put_bytes;
327  t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte;
328  t->trans_tx.start_message = (start_message_t) start_message;
329  t->trans_tx.end_message = (end_message_t) end_message;
330  t->trans_tx.overrun = (overrun_t) overrun;
331  t->trans_tx.count_bytes = (count_bytes_t) count_bytes;
332  t->trans_tx.impl = (void *)(t);
333  PPRZ_MUTEX_INIT(t->mtx_tx); // init mutex, check if correct pointer
334 
335  // add whitelist messages
338 }
339 
340 void gec_dl_init(void)
341 {
342 #ifdef GEC_STATUS_LED
343  LED_OFF(GEC_STATUS_LED);
344 #endif
345 
346  // init pprz transport
347  pprz_transport_init(&gec_tp.pprz_tp);
348 
349  // init crypto transport
351 
352  // initialize keys
354 
355 #if PERIODIC_TELEMETRY
356  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SECURE_LINK_STATUS, send_secure_link_info);
357 #endif
358 }
359 
366 bool gec_encrypt_message(uint8_t *buf, uint8_t *payload_len)
367 {
368  // check payload length
369  if (*payload_len <= PPRZ_MSG_ID) {
370  return false;
371  }
372  // check if the key is ready
373  if (!gec_tp.sts.tx_sym_key.ready) {
374  return false;
375  }
376 
377  // increment counter
379  // convert to bytes
380  uint8_t counter_as_bytes[4];
381  gec_counter_to_bytes(counter, counter_as_bytes);
382  // update nonce
383  memcpy(gec_tp.sts.tx_sym_key.nonce, counter_as_bytes, sizeof(uint32_t));
384 
385  // update intermediate fields
386  uint8_t auth[PPRZ_AUTH_LEN];
387  memcpy(auth, buf, PPRZ_AUTH_LEN);
388  uint8_t tag[PPRZ_MAC_LEN];
389  uint8_t ciphertext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
390  uint32_t mlen = *payload_len - PPRZ_AUTH_LEN; // at least CLASS_BYTE and MSG_ID
391 
392  uint32_t res = Hacl_Chacha20Poly1305_aead_encrypt(ciphertext, tag,
393  &buf[PPRZ_AUTH_LEN], mlen, auth,
394  PPRZ_AUTH_LEN, gec_tp.sts.tx_sym_key.key, gec_tp.sts.tx_sym_key.nonce);
395 
396  if (res == 0) {
397  memset(buf, 0, TRANSPORT_PAYLOAD_LEN); // reset whole buffer
398  *payload_len = 0;
399  buf[PPRZ_GEC_IDX] = PPRZ_MSG_TYPE_ENCRYPTED; // CRYPTO_BYTE
400  *payload_len += 1;
401  memcpy(&buf[PPRZ_CNTR_IDX], counter_as_bytes, sizeof(uint32_t)); // counter
402  *payload_len += sizeof(uint32_t);
403  memcpy(&buf[PPRZ_AUTH_IDX], auth, PPRZ_AUTH_LEN); // auth data
404  *payload_len += PPRZ_AUTH_LEN;
405  memcpy(&buf[PPRZ_CIPH_IDX], ciphertext, mlen); // ciphertext
406  *payload_len += mlen;
407  memcpy(&buf[PPRZ_CIPH_IDX + mlen], tag, PPRZ_MAC_LEN); // tag
408  *payload_len += PPRZ_MAC_LEN;
409  gec_tp.sts.tx_sym_key.counter = counter; // update counter
410  return true;
411  } else {
413  return false;
414  }
415 }
416 
425 bool gec_decrypt_message(uint8_t *buf, volatile uint8_t *payload_len)
426 {
427  switch (buf[PPRZ_GEC_IDX]) {
429  // handle plaintext message
430  if (*payload_len < PPRZ_PLAINTEXT_MSG_MIN_LEN) {
431  return false;
432  }
433  // check if the message is in the whitelist
435  buf[PPRZ_PLAINTEXT_MSG_ID_IDX])) {
436  // return the buffer minus the crypto byte
437  memmove(buf, &buf[1], *payload_len - 1);
438  return true;
439  }
440  return false;
441  break;
443  // attempt decryption
444  if (*payload_len < PPRZ_ENCRYPTED_MSG_MIN_LEN) {
445  return false;
446  }
447  if (!gec_tp.sts.rx_sym_key.ready) {
448  // the rx key is not ready yet
449  return false;
450  }
451  // first check the message counter
453  // check against the saved counter
454  if (counter <= gec_tp.sts.rx_sym_key.counter) {
456  return false;
457  }
458  // update nonce with 4 counter bytes
459  memcpy(gec_tp.sts.rx_sym_key.nonce, &buf[PPRZ_CNTR_IDX],
460  sizeof(uint32_t));
461  // update intermediate fields
462  uint8_t auth[PPRZ_AUTH_LEN];
463  memcpy(auth, &buf[PPRZ_AUTH_IDX], PPRZ_AUTH_LEN);
464  uint8_t tag[PPRZ_MAC_LEN];
465  memcpy(tag, &buf[*payload_len - PPRZ_MAC_LEN], PPRZ_MAC_LEN);
466  uint8_t ciphertext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
467  // payload - AUTH_DATA(2) - TAG(16) - CRYPTO_BYTE(1) - COUNTER(4) = payload_len - 23
468  uint32_t mlen = *payload_len - PPRZ_CRYPTO_OVERHEAD - 1 - PPRZ_AUTH_LEN; // don't forget to count for CRYPTO_BYTE
469  memcpy(ciphertext, &buf[PPRZ_CIPH_IDX], mlen);
470  uint8_t plaintext[TRANSPORT_PAYLOAD_LEN - PPRZ_CRYPTO_OVERHEAD];
471 
472  // try decryption
473  uint32_t res = Hacl_Chacha20Poly1305_aead_decrypt(plaintext, ciphertext,
474  mlen, tag, auth,
475  PPRZ_AUTH_LEN, gec_tp.sts.rx_sym_key.key,
477 
478  // res == 0 means all good
479  if (res == 0) {
480  memset(buf, 0, TRANSPORT_PAYLOAD_LEN); // reset whole buffer
481  *payload_len = 0;
482  memcpy(buf, auth, PPRZ_AUTH_LEN); // copy auth data
483  *payload_len += PPRZ_AUTH_LEN;
484  memcpy(&buf[PPRZ_AUTH_LEN], plaintext, mlen); // copy plaintext
485  *payload_len += mlen;
486  gec_tp.sts.rx_sym_key.counter = counter; // update counter
487  return true;
488  } else {
490  return false;
491  }
492  break;
493  default:
494  break;
495  }
496  return false;
497 }
498 
528 void gec_dl_event(void)
529 {
530  // strip PPRZ_STX, MSG_LEN, and CHEKSUM, return (CRYPTO_BYTE..MSG_PAYLOAD)
531  pprz_check_and_parse(&DOWNLINK_DEVICE.device, &gec_tp.pprz_tp,
532  gec_tp.pprz_tp.trans_rx.payload, (bool *) &gec_tp.trans_rx.msg_received);
533  // we have (CRYPTO_BYTE .. MSG_PAYLOAD) in
534  if (gec_tp.trans_rx.msg_received) { // self.rx.parse_byte(b)
535  switch (gec_tp.sts.protocol_stage) {
536  case CRYPTO_OK:
537  // decrypt message
538  // if successfull return the message (sender_ID .. MSG_payload)
539  if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
540  &gec_tp.pprz_tp.trans_rx.payload_len)) {
541  // copy the buffer over
542  // NOTE:the real payload_len is at least one byte shorter
543  // but we can copy whole buffer anyway since we don't overflow
544  DatalinkFillDlBuffer(gec_tp.pprz_tp.trans_rx.payload,
545  gec_tp.pprz_tp.trans_rx.payload_len);
546  // pass to datalink
549  }
550  break;
551  case WAIT_MSG1:
552  // decrypt message (checks against the whitelist and removes the crypto byte)
553  // returns (SENDER_ID .. MSG_PAYLOAD
554  if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
555  &gec_tp.pprz_tp.trans_rx.payload_len)) {
556  gec_process_msg1(gec_tp.pprz_tp.trans_rx.payload);
557  } else {
558  }
559  break;
560  case WAIT_MSG3:
561  // decrypt message (checks against the whitelist and removes the crypto byte)
562  // returns (SENDER_ID .. MSG_PAYLOAD
563  if (gec_decrypt_message(gec_tp.pprz_tp.trans_rx.payload,
564  &gec_tp.pprz_tp.trans_rx.payload_len)) {
565  if (gec_process_msg3(gec_tp.pprz_tp.trans_rx.payload)) {
566  // if OK, we are ready for an ongoing communication
568 #ifdef GEC_STATUS_LED
569  LED_ON(GEC_STATUS_LED);
570 #endif
571  }
572  }
573  break;
574  default:
575  // shouldn't be here unless something is really wrong
576  break;
577  }
578  // reset flag
579  gec_tp.trans_rx.msg_received = false;
580  }
581 }
582 
593 {
594  if (SenderIdOfPprzMsg(buf) != 0) {
595  // process only messages from GCS
596  // log an error
597  return;
598  }
599 
600  // check message ID
601  if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
603  return;
604  }
605 
606  // check message type
607  if (DL_KEY_EXCHANGE_GCS_msg_type(buf) != P_AE) {
609  return;
610  }
611 
612  // check data length
613  if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf) != PPRZ_KEY_LEN) {
615  return;
616  }
617 
618  // copy P_ae over
620  DL_KEY_EXCHANGE_GCS_msg_data(buf), sizeof(struct gec_pubkey));
622 
623  // 2. B generates ephemeral curve25519 key pair (Pbe, Qbe).
626  }
627 
628  // 3. B computes the shared secret: z = scalar_multiplication(Qbe, Pae)
629  uint8_t z[32] = { 0 };
630  Hacl_Curve25519_crypto_scalarmult(z, gec_tp.sts.my_private_ephemeral.priv,
632 
633  // 4. B uses the key derivation function kdf(z,1) to compute Kb || Sb,
634  // kdf(z,0) to compute Ka || Sa, and kdf(z,2) to compute Kclient || Sclient.
636 
637  // 5. B computes the ed25519 signature: sig = signQb(Pbe || Pae)
638  uint8_t sig[PPRZ_SIGN_LEN] = { 0 };
639  uint8_t pbe_concat_p_ae[PPRZ_KEY_LEN * 2] = { 0 };
640  memcpy(pbe_concat_p_ae, &gec_tp.sts.my_private_ephemeral.pub, PPRZ_KEY_LEN);
641  memcpy(&pbe_concat_p_ae[PPRZ_KEY_LEN], &gec_tp.sts.their_public_ephemeral.pub,
642  PPRZ_KEY_LEN);
643  Hacl_Ed25519_sign(sig, gec_tp.sts.my_private_key.priv, pbe_concat_p_ae,
644  PPRZ_KEY_LEN * 2);
645 
646  // 6. B computes and sends the message Pbe || Ekey=Kb,IV=Sb||zero(sig)
647  uint8_t msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN + PPRZ_MAC_LEN] = { 0 };
648  memcpy(msg_data, &gec_tp.sts.my_private_ephemeral.pub, PPRZ_KEY_LEN);
649 
650  if (Hacl_Chacha20Poly1305_aead_encrypt(&msg_data[PPRZ_KEY_LEN],
651  &msg_data[PPRZ_KEY_LEN + PPRZ_SIGN_LEN], sig, PPRZ_SIGN_LEN, NULL, 0,
654  return;
655  }
656 
657 #if PPRZLINK_DEFAULT_VER == 2
658  // now we have to manually construct the message
659  // CRYPTO BYTE
660  // source_id
661  // sender_id
662  // class id
663  // msg id
664  // msg_type
665  // msg_data
667  gec_tp.tx_msg_idx = 1;
668  gec_tp.tx_msg[gec_tp.tx_msg_idx] = AC_ID;
669  gec_tp.tx_msg_idx++;
671  gec_tp.tx_msg_idx++;
672  gec_tp.tx_msg[gec_tp.tx_msg_idx] = 1;// telemetry
673  gec_tp.tx_msg_idx++;
675  gec_tp.tx_msg_idx++;
677  gec_tp.tx_msg_idx++;
678  gec_tp.tx_msg[gec_tp.tx_msg_idx] = sizeof(msg_data);
679  gec_tp.tx_msg_idx++;
680  memcpy(&gec_tp.tx_msg[gec_tp.tx_msg_idx], msg_data, sizeof(msg_data));
681  gec_tp.tx_msg_idx += sizeof(msg_data);
682 
683  struct pprzlink_msg msg2;
684  msg2.trans = &gec_tp.pprz_tp.trans_tx;
685  msg2.dev = &DOWNLINK_DEVICE.device;
686 
687  gec_encapsulate_and_send_msg(&msg2, 0);
688 #else
689 #if PPRZLINK_DEFAULT_VER ==1
690  // Pprzlink 1.0
691  // now we have to manually construct the message
692  // CRYPTO BYTE
693  // source_id
694  // msg id
695  // msg_type
696  // msg_data
698  gec_tp.tx_msg_idx = 1;
699  gec_tp.tx_msg[gec_tp.tx_msg_idx] = AC_ID;
700  gec_tp.tx_msg_idx++;
702  gec_tp.tx_msg_idx++;
704  gec_tp.tx_msg_idx++;
705  gec_tp.tx_msg[gec_tp.tx_msg_idx] = sizeof(msg_data);
706  gec_tp.tx_msg_idx++;
707  memcpy(&gec_tp.tx_msg[gec_tp.tx_msg_idx], msg_data, sizeof(msg_data));
708  gec_tp.tx_msg_idx += sizeof(msg_data);
709 
710  gec_encapsulate_and_send_msg(&gec_tp, &DOWNLINK_DEVICE.device, 0);
711 #endif // PPRZLINK 1.0
712 #endif // PPRZLINK 2.0
713 
714  /*
715  * Note that ideally we would use the following function call, but the problem is that
716  * 'end_message()` cannot currently distinguish between encrypted and plaintext message to be sent
717  * (i.e. I don't know the message ID). The second problem I am using the same buffer here as for
718  * constructing a message, so a corruption of the buffer is possible.
719  *
720  * Since I am doing this only once, the method above is fine for the time being.
721  *
722  * Pprzlink 2.0:
723  uint8_t msg_status = P_BE;
724  pprz_msg_send_KEY_EXCHANGE_UAV(&gec_tp.pprz_tp.trans_tx, &DOWNLINK_DEVICE.device, AC_ID,
725  &msg_status, sizeof(msg_data), msg_data);
726  */
727 
728  // increment status
730 }
731 
740 {
741  if (SenderIdOfPprzMsg(buf) != 0) {
742  // process only messages from GCS
743  // log an error
744  return false;
745  }
746 
747 // check message ID
748  if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
750  return false;
751  }
752 
753 // check message type
754  if (DL_KEY_EXCHANGE_GCS_msg_type(buf) != SIG) {
756  return false;
757  }
758 
759 // check data length
760  if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf)
761  != (PPRZ_SIGN_LEN + PPRZ_MAC_LEN)) {
763  return false;
764  }
765 
766  // 13. B decrypts the message and verifies the signature.
767  uint8_t sign[PPRZ_SIGN_LEN] = { 0 };
768  if (Hacl_Chacha20Poly1305_aead_decrypt(sign,
769  DL_KEY_EXCHANGE_GCS_msg_data(buf),
770  PPRZ_SIGN_LEN, DL_KEY_EXCHANGE_GCS_msg_data(buf) + PPRZ_SIGN_LEN, NULL, 0,
773  return false;
774  }
775 
776  // verify
777  uint8_t p_ae_concat_p_be[PPRZ_KEY_LEN * 2] = { 0 };
778  memcpy(p_ae_concat_p_be, &gec_tp.sts.their_public_ephemeral.pub,
779  PPRZ_KEY_LEN);
780  memcpy(&p_ae_concat_p_be[PPRZ_KEY_LEN], &gec_tp.sts.my_private_ephemeral.pub,
781  PPRZ_KEY_LEN);
782  // returns true if verified properly
783  if (!Hacl_Ed25519_verify(gec_tp.sts.their_public_key.pub, p_ae_concat_p_be,
784  PPRZ_SIGN_LEN, sign)) {
785  // log error and return
787  return false;
788  }
789 
790  // all ok
791  return true;
792 }
793 
794 // add a message id to the whitelist
795 void gec_add_to_whitelist(struct gec_whitelist *whitelist, uint8_t id)
796 {
797  if (whitelist->init && (whitelist->idx < WHITELIST_LEN)) {
798  whitelist->whitelist[whitelist->idx] = id;
799  whitelist->idx++;
800 
801  } else {
802  memset(whitelist, 0, WHITELIST_LEN); // erase the whitelist
803  whitelist->init = true;
804  whitelist->idx = 0;
805  }
806 }
807 
808 // return true if given message id is in the whitelist
809 bool gec_is_in_the_whitelist(struct gec_whitelist *whitelist, uint8_t id)
810 {
811  if (whitelist->init) {
812  for (uint8_t i = 0; i < whitelist->idx; i++) {
813  if (whitelist->whitelist[i] == id) {
814  return true;
815  }
816  }
817  }
818  return false;
819 }
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
#define GEC_UPDATE_DL
Definition: gec_dl.c:67
bool gec_is_in_the_whitelist(struct gec_whitelist *whitelist, uint8_t id)
Definition: gec_dl.c:809
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:340
#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:795
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:75
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:70
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:366
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:425
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:320
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:74
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:739
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:592
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:89
#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:528
Definition: gec.h:91