Git-Mirrors/OpenModem
1
0
Fork 0
mirror of https://github.com/markqvist/OpenModem.git synced 2025-06-05 21:38:51 -04:00
Code Issues Projects Releases Packages Wiki Activity Actions

Implemented AES-128 encryption

Browse source
This commit is contained in:
Mark Qvist 2019-02-07 18:36:40 +01:00
parent 0e24b54657
commit b3b1a9b253
15 changed files with 730 additions and 118 deletions
Download patch file Download diff file Expand all files Collapse all files

179
protocol/KISS.c
View file

@ -4,6 +4,7 @@
#include "device.h"
#include "hardware/Serial.h"
#include "hardware/LED.h"
#include "hardware/Crypto.h"
#include "util/FIFO16.h"
#include "util/time.h"
#include "KISS.h"
@ -65,21 +66,76 @@ void kiss_messageCallback(AX25Ctx *ctx) {
decodes++;
printf("%d\r\n", decodes);
#else
fputc(FEND, &serial->uart0);
fputc(0x00, &serial->uart0);
for (unsigned i = 0; i < ctx->frame_len-2; i++) {
uint8_t b = ctx->buf[i];
if (b == FEND) {
fputc(FESC, &serial->uart0);
fputc(TFEND, &serial->uart0);
} else if (b == FESC) {
fputc(FESC, &serial->uart0);
fputc(TFESC, &serial->uart0);
} else {
fputc(b, &serial->uart0);
bool integrity_ok = false;
if (crypto_enabled()) {
size_t rxpos = 0;
// Get padding size
uint8_t padding = ctx->buf[rxpos++];
size_t data_length = ctx->frame_len - 2 - 1 - CRYPTO_HMAC_SIZE - CRYPTO_KEY_SIZE;
size_t hmac_offset = ctx->frame_len - 2 - CRYPTO_HMAC_SIZE;
// Get HMAC
uint8_t hmac[CRYPTO_HMAC_SIZE];
memset(hmac, 0x00, CRYPTO_HMAC_SIZE);
for (uint8_t i = 0; i < CRYPTO_HMAC_SIZE; i++) {
size_t pos = hmac_offset + i;
hmac[i] = ctx->buf[pos];
}
// Calculate HMAC
crypto_generate_hmac(ctx->buf, ctx->frame_len-2-CRYPTO_HMAC_SIZE);
bool HMAC_ok = true;
for (uint8_t i = 0; i < CRYPTO_HMAC_SIZE; i++) {
if (hmac[i] != crypto_work_block[i]) {
HMAC_ok = false;
}
}
if (HMAC_ok) {
// Get IV
for (uint8_t i = 0; i < CRYPTO_KEY_SIZE; i++) {
crypto_work_block[i] = ctx->buf[rxpos++];
}
crypto_set_iv_from_workblock();
crypto_prepare();
uint8_t blocks = data_length / CRYPTO_KEY_SIZE;
size_t decrypted_pos = 0;
for (uint8_t block = 0; block < blocks; block++) {
for (uint8_t i = 0; i < CRYPTO_KEY_SIZE; i++) {
crypto_work_block[i] = ctx->buf[rxpos++];
}
crypto_decrypt_block(crypto_work_block);
for (uint8_t i = 0; i < CRYPTO_KEY_SIZE; i++) {
ctx->buf[decrypted_pos++] = crypto_work_block[i];
}
}
ctx->frame_len = data_length - padding;
integrity_ok = true;
}
} else {
integrity_ok = true;
}
if (integrity_ok) {
fputc(FEND, &serial->uart0);
fputc(0x00, &serial->uart0);
for (unsigned i = 0; i < ctx->frame_len-2; i++) {
uint8_t b = ctx->buf[i];
if (b == FEND) {
fputc(FESC, &serial->uart0);
fputc(TFEND, &serial->uart0);
} else if (b == FESC) {
fputc(FESC, &serial->uart0);
fputc(TFESC, &serial->uart0);
} else {
fputc(b, &serial->uart0);
}
}
fputc(FEND, &serial->uart0);
}
fputc(FEND, &serial->uart0);
#endif
}
@ -110,24 +166,6 @@ void kiss_csma(void) {
}
}
// TODO: Remove this
// void kiss_flushQueueDebug(void) {
// printf("Queue height %d\r\n", queue_height);
// for (size_t n = 0; n < queue_height; n++) {
// size_t start = fifo16_pop(&packet_starts);
// size_t length = fifo16_pop(&packet_lengths);
// printf("--- Packet %d, %d bytes ---\r\n", n+1, length);
// for (size_t i = 0; i < length; i++) {
// size_t pos = (start+i)%CONFIG_QUEUE_SIZE;
// printf("%02x", packet_queue[pos]);
// }
// printf("\r\n\r\n");
// }
// queue_height = 0;
// queued_bytes = 0;
// }
volatile bool queue_flushing = false;
void kiss_flushQueue(void) {
if (!queue_flushing) {
@ -138,14 +176,74 @@ void kiss_flushQueue(void) {
size_t start = fifo16_pop_locked(&packet_starts);
size_t length = fifo16_pop_locked(&packet_lengths);
//kiss_poll();
for (size_t i = 0; i < length; i++) {
size_t pos = (start+i)%CONFIG_QUEUE_SIZE;
tx_buffer[i] = packet_queue[pos];
}
if (crypto_enabled()) {
uint8_t padding = CRYPTO_KEY_SIZE - (length % CRYPTO_KEY_SIZE);
if (padding == CRYPTO_KEY_SIZE) padding = 0;
ax25_sendRaw(ax25ctx, tx_buffer, length);
processed++;
uint8_t blocks = (length + padding) / CRYPTO_KEY_SIZE;
if (crypto_generate_iv()) {
crypto_prepare();
size_t tx_pos = 0;
tx_buffer[tx_pos++] = padding;
uint8_t *iv = crypto_get_iv();
for (uint8_t i = 0; i < CRYPTO_KEY_SIZE; i++) {
tx_buffer[tx_pos++] = iv[i];
}
// Encrypt each block
for (uint8_t i = 0; i < blocks; i++) {
if (i < blocks-1 || padding == 0) {
for (uint8_t j = 0; j < CRYPTO_KEY_SIZE; j++) {
size_t pos = (start+j)%CONFIG_QUEUE_SIZE;
crypto_work_block[j] = packet_queue[pos];
}
start += CRYPTO_KEY_SIZE;
} else {
for (uint8_t j = 0; j < CRYPTO_KEY_SIZE - padding; j++) {
size_t pos = (start+j)%CONFIG_QUEUE_SIZE;
crypto_work_block[j] = packet_queue[pos];
}
for (uint8_t j = 0; j < padding; j++) {
crypto_work_block[j] = 0xFF;
}
}
crypto_encrypt_block(crypto_work_block);
for (uint8_t j = 0; j < CRYPTO_KEY_SIZE; j++) {
tx_buffer[tx_pos++] = crypto_work_block[j];
}
}
// Genereate MAC
crypto_generate_hmac(tx_buffer, tx_pos);
for (uint8_t i = 0; i < CRYPTO_HMAC_SIZE; i++) {
tx_buffer[tx_pos++] = crypto_work_block[i];
}
// Check size and send
if (tx_pos <= AX25_MAX_FRAME_LEN) {
ax25_sendRaw(ax25ctx, tx_buffer, tx_pos);
processed++;
} else {
processed++;
}
} else {
LED_indicate_error_crypto();
}
} else {
for (size_t i = 0; i < length; i++) {
size_t pos = (start+i)%CONFIG_QUEUE_SIZE;
tx_buffer[i] = packet_queue[pos];
}
ax25_sendRaw(ax25ctx, tx_buffer, length);
processed++;
}
}
if (processed < queue_height) {
@ -181,7 +279,7 @@ void kiss_serialCallback(uint8_t sbyte) {
IN_FRAME = true;
command = CMD_UNKNOWN;
frame_len = 0;
} else if (IN_FRAME && frame_len < AX25_MAX_FRAME_LEN) {
} else if (IN_FRAME && frame_len < AX25_MAX_PAYLOAD) {
// Have a look at the command byte first
if (frame_len == 0 && command == CMD_UNKNOWN) {
// OpenModem supports only one HDLC port, so we
@ -214,9 +312,6 @@ void kiss_serialCallback(uint8_t sbyte) {
p = sbyte;
} else if (command == CMD_FLUSHQUEUE) {
kiss_flushQueue();
// TODO: Remove this
//} else if (command == CMD_FLUSHQUEUE_DEBUG) {
// kiss_flushQueueDebug();
} else if (command == CMD_LED_INTENSITY) {
if (sbyte == FESC) {
ESCAPE = true;