29#include "pprzlink/messages.h"
32#if PPRZLINK_DEFAULT_VER == 2
36#define PPRZ_PLAINTEXT_MSG_ID_IDX 4
38#define PPRZ_PLAINTEXT_MSG_MIN_LEN 5
40#define PPRZ_ENCRYPTED_MSG_MIN_LEN 25
42#define PPRZ_AUTH_LEN 2
44#define PPRZ_CIPH_IDX 7
46#if PPRZLINK_DEFAULT_VER == 1
50#define PPRZ_PLAINTEXT_MSG_ID_IDX 2
52#define PPRZ_PLAINTEXT_MSG_MIN_LEN 3
54#define PPRZ_ENCRYPTED_MSG_MIN_LEN 23
56#define PPRZ_AUTH_LEN 1
58#define PPRZ_CIPH_IDX 6
67#define GEC_UPDATE_DL TRUE
77 pprz_msg_send_SECURE_LINK_STATUS(trans,
dev, AC_ID,
80 &
gec_tp.sts.rx_counter_err,
95#if PPRZLINK_DEFAULT_VER == 2
96void gec_encapsulate_and_send_msg(
struct pprzlink_msg *msg,
long fd);
101static struct gec_transport *get_trans(
struct pprzlink_msg *msg)
109static 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,
116 for (i = 0; i < len; i++) {
121static 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)))
137 + get_trans(msg)->pprz_tp.trans_tx.size_of(msg, len);
143static void start_message(
struct pprzlink_msg *msg,
144 long fd __attribute__((unused)),
145 uint8_t payload_len __attribute__((unused)))
148 memset(get_trans(msg)->
tx_msg, 0, TRANSPORT_PAYLOAD_LEN);
149 get_trans(msg)->tx_msg_idx = 0;
155static void overrun(
struct pprzlink_msg *msg)
157 get_trans(msg)->pprz_tp.trans_tx.overrun(msg);
163static void count_bytes(
struct pprzlink_msg *msg,
uint8_t bytes)
165 get_trans(msg)->pprz_tp.trans_tx.count_bytes(msg, bytes);
177static int check_available_space(
struct pprzlink_msg *msg,
long *
fd,
181 return get_trans(msg)->pprz_tp.trans_tx.check_available_space(msg,
fd,
194static void end_message(
struct pprzlink_msg *msg,
long fd)
196 switch (
gec_tp.sts.protocol_stage) {
199 gec_encapsulate_and_send_msg(msg,
fd);
216void gec_encapsulate_and_send_msg(
struct pprzlink_msg *msg,
long fd)
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,
223 get_trans(msg)->pprz_tp.trans_tx.end_message(msg,
fd);
227#if PPRZLINK_DEFAULT_VER == 1
228void gec_encapsulate_and_send_msg(
struct gec_transport *trans,
229 struct link_device *
dev,
long fd);
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,
240 for (i = 0; i < len; i++) {
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)))
262 struct link_device *
dev __attribute__((unused)),
263 long fd __attribute__((unused)),
264 uint8_t payload_len __attribute__((unused)))
267 memset(trans->
tx_msg, 0, TRANSPORT_PAYLOAD_LEN);
271static void end_message(
struct gec_transport *trans,
struct link_device *
dev,
274 switch (
gec_tp.sts.protocol_stage) {
277 gec_encapsulate_and_send_msg(trans,
dev,
fd);
289void gec_encapsulate_and_send_msg(
struct gec_transport *trans,
290 struct link_device *
dev,
long fd)
293 trans->
pprz_tp.trans_tx.put_bytes(trans,
dev,
fd, DL_TYPE_UINT8,
300static void overrun(
struct gec_transport *trans __attribute__((unused)),
301 struct link_device *
dev)
306static void count_bytes(
struct gec_transport *trans __attribute__((unused)),
309 trans->
pprz_tp.trans_tx.count_bytes(trans,
dev, bytes);
312static int check_available_space(
317 return trans->
pprz_tp.trans_tx.check_available_space(trans,
dev,
fd, bytes);
328 t->
trans_tx.size_of = (size_of_t) size_of;
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;
352 pprz_transport_init(&
gec_tp.pprz_tp);
360#if PERIODIC_TELEMETRY
374 if (*payload_len <= PPRZ_MSG_ID) {
378 if (!
gec_tp.sts.tx_sym_key.ready) {
388 memcpy(
gec_tp.sts.tx_sym_key.nonce, counter_as_bytes,
sizeof(
uint32_t));
392 memcpy(auth, buf, PPRZ_AUTH_LEN);
395 uint32_t mlen = *payload_len - PPRZ_AUTH_LEN;
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);
402 memset(buf, 0, TRANSPORT_PAYLOAD_LEN);
409 *payload_len += PPRZ_AUTH_LEN;
410 memcpy(&buf[PPRZ_CIPH_IDX], ciphertext, mlen);
411 *payload_len += mlen;
435 if (*payload_len < PPRZ_PLAINTEXT_MSG_MIN_LEN) {
440 buf[PPRZ_PLAINTEXT_MSG_ID_IDX])) {
442 memmove(buf, &buf[1], *payload_len - 1);
449 if (*payload_len < PPRZ_ENCRYPTED_MSG_MIN_LEN) {
452 if (!
gec_tp.sts.rx_sym_key.ready) {
460 gec_tp.sts.rx_counter_err++;
474 memcpy(ciphertext, &buf[PPRZ_CIPH_IDX], mlen);
478 uint32_t res = Hacl_Chacha20Poly1305_aead_decrypt(plaintext, ciphertext,
480 PPRZ_AUTH_LEN,
gec_tp.sts.rx_sym_key.key,
481 gec_tp.sts.rx_sym_key.nonce);
485 memset(buf, 0, TRANSPORT_PAYLOAD_LEN);
487 memcpy(buf, auth, PPRZ_AUTH_LEN);
488 *payload_len += PPRZ_AUTH_LEN;
489 memcpy(&buf[PPRZ_AUTH_LEN], plaintext, mlen);
490 *payload_len += mlen;
537 gec_tp.pprz_tp.trans_rx.payload, (
bool *) &
gec_tp.trans_rx.msg_received);
539 if (
gec_tp.trans_rx.msg_received) {
540 switch (
gec_tp.sts.protocol_stage) {
545 &
gec_tp.pprz_tp.trans_rx.payload_len)) {
550 gec_tp.pprz_tp.trans_rx.payload_len);
560 &
gec_tp.pprz_tp.trans_rx.payload_len)) {
569 &
gec_tp.pprz_tp.trans_rx.payload_len)) {
584 gec_tp.trans_rx.msg_received =
false;
599 if (SenderIdOfPprzMsg(buf) != 0) {
606 if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
612 if (DL_KEY_EXCHANGE_GCS_msg_type(buf) !=
P_AE) {
618 if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf) !=
PPRZ_KEY_LEN) {
624 memcpy(&
gec_tp.sts.their_public_ephemeral.pub,
625 DL_KEY_EXCHANGE_GCS_msg_data(buf),
sizeof(
struct gec_pubkey));
626 gec_tp.sts.their_public_ephemeral.ready =
true;
629 if (!
gec_tp.sts.my_private_ephemeral.ready) {
635 Hacl_Curve25519_crypto_scalarmult(z,
gec_tp.sts.my_private_ephemeral.priv,
636 gec_tp.sts.their_public_ephemeral.pub);
648 Hacl_Ed25519_sign(sig,
gec_tp.sts.my_private_key.priv, pbe_concat_p_ae,
655 if (Hacl_Chacha20Poly1305_aead_encrypt(&msg_data[
PPRZ_KEY_LEN],
657 gec_tp.sts.tx_sym_key.key,
gec_tp.sts.tx_sym_key.nonce)) {
662#if PPRZLINK_DEFAULT_VER == 2
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);
688 struct pprzlink_msg msg2;
689 msg2.trans = &
gec_tp.pprz_tp.trans_tx;
692 gec_encapsulate_and_send_msg(&msg2, 0);
694#if PPRZLINK_DEFAULT_VER ==1
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);
746 if (SenderIdOfPprzMsg(buf) != 0) {
753 if (IdOfPprzMsg(buf) != DL_KEY_EXCHANGE_GCS) {
759 if (DL_KEY_EXCHANGE_GCS_msg_type(buf) !=
SIG) {
765 if (DL_KEY_EXCHANGE_GCS_msg_data_length(buf)
773 if (Hacl_Chacha20Poly1305_aead_decrypt(
sign,
774 DL_KEY_EXCHANGE_GCS_msg_data(buf),
776 gec_tp.sts.rx_sym_key.key,
gec_tp.sts.rx_sym_key.nonce)) {
783 memcpy(p_ae_concat_p_be, &
gec_tp.sts.their_public_ephemeral.pub,
788 if (!Hacl_Ed25519_verify(
gec_tp.sts.their_public_key.pub, p_ae_concat_p_be,
808 whitelist->
init =
true;
816 if (whitelist->
init) {
817 for (
uint8_t i = 0; i < whitelist->
idx; i++) {
int pprz_mtx_unlock(pprz_mutex_t *mtx)
int pprz_mtx_init(pprz_mutex_t *mtx)
int pprz_mtx_lock(pprz_mutex_t *mtx)
uint8_t * datalink_get_buffer(void)
get datalink buffer address
void datalink_fill_buffer(uint8_t *buf, uint16_t len)
fill datalink buffer
Handling of messages coming from ground and other A/Cs.
static void DlCheckAndParse(struct link_device *dev, struct transport_tx *trans, uint8_t *buf, bool *msg_available, bool update_dl)
Check for new message and parse.
static struct uart_periph * dev
void gec_counter_to_bytes(uint32_t n, uint8_t *bytes)
Convert counter to bytes in network byte order.
void gec_sts_init(struct gec_sts_ctx *sts)
void gec_generate_ephemeral_keys(struct gec_privkey *sk)
Generate private and public key pairs for future use.
void gec_derive_key_material(struct gec_sts_ctx *ctx, uint8_t *z)
Derive key material for both sender and receiver.
uint32_t gec_bytes_to_counter(uint8_t *bytes)
Convert from network byte order (big endian) to the machine byte order.
Galois embedded crypto implementation.
#define PPRZ_CRYPTO_OVERHEAD
@ UNEXPECTED_MSG_TYPE_ERROR
@ UNEXPECTED_MSG_DATA_ERROR
#define PPRZ_MSG_TYPE_ENCRYPTED
#define PPRZ_MSG_TYPE_PLAINTEXT
void gec_dl_init(void)
Init function.
static void send_secure_link_info(struct transport_tx *trans, struct link_device *dev)
bool gec_process_msg3(uint8_t *buf)
Process incoming message (expected MSG3) if the right (KEY_EXCHANGE) message received with the right ...
void gec_add_to_whitelist(struct gec_whitelist *whitelist, uint8_t id)
void gec_dl_event(void)
Parse incoming message bytes (PPRZ_STX..CHCKSUM B) and returns a new decrypted message if it is avail...
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...
struct gec_transport gec_tp
PPRZ transport structure.
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)
Attemp message decryption If a message is unencrypted, pass it through only if the MSG_ID is in the w...
void gec_process_msg1(uint8_t *buf)
NOTE: for RESPONDER party only Process incoming message (expected MSG1) if the right (KEY_EXCHANGE) m...
static void insert_byte(struct gec_transport *t, const uint8_t byte)
Simply insert byte to the message buffer.
void gec_transport_init(struct gec_transport *t)
Datalink using Galois Embedded Crypto.
#define KEY_EXCHANGE_MSG_ID_GCS
uint8_t whitelist[WHITELIST_LEN]
uint8_t tx_msg[TRANSPORT_PAYLOAD_LEN]
struct transport_tx trans_tx
#define KEY_EXCHANGE_MSG_ID_UAV
struct transport_rx trans_rx
struct pprz_transport pprz_tp
struct gec_whitelist whitelist
Whitelist for sending and receiving unencrypted messages (mostly for KEY_EXCHANGE messages).
arch independent LED (Light Emitting Diodes) API
bool dl_msg_available
Flag provided to control calls to dl_parse_msg.
static float sign(float x)
sign function
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.