Skip to content

Commit

Permalink
* Formatting of some of the code tidied up
Browse files Browse the repository at this point in the history 
* Sending response to RT commands to modem encrypted...like standard data
* Fix to Encryption routine so that injected packets are corrupted by the padding routine.
  • Loading branch information
@joeman155
joeman155 committed Aug 8, 2015
1 parent b29da99 commit 2883998
Show file tree
Hide file tree
Showing 4 changed files with 49 additions and 30 deletions.
14 changes: 11 additions & 3 deletions Firmware/radio/AES/aes.c
View file
Expand Up @@ -35,6 +35,8 @@
#include "CTR_EncryptDecrypt.h"
#include <stdlib.h>

#define MAX_ENCRYPT_PACKET_LENGTH 32

/* SEGMENT_VARIABLE (EncryptionKey[32], U8, SEG_XDATA); */
__xdata unsigned char *EncryptionKey;
SEGMENT_VARIABLE (DecryptionKey[32], U8, SEG_XDATA);
Expand All @@ -46,6 +48,7 @@ SEGMENT_VARIABLE (Counter[16], U8, SEG_XDATA);
const SEGMENT_VARIABLE (Nonce[16], U8, SEG_CODE) = {0xf0, 0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa, 0xfb, 0xfc, 0xfd, 0xfe, 0xff};
const SEGMENT_VARIABLE (ReferenceInitialVector[16] , U8, SEG_CODE) = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};

static __xdata uint8_t encrypt_packet[MAX_ENCRYPT_PACKET_LENGTH];

/* Helper definitions */
// First nibble = code for # of bits - 1 = 128, 2 = 192, 3 = 256
Expand Down Expand Up @@ -167,13 +170,18 @@ __xdata unsigned char *aes_pad(__xdata unsigned char *in_str, uint8_t len)
volatile uint8_t pad_length;
uint8_t i;

// Copy string to encrypt to temp area, because we will be padding out encrypted string
// and we don't want to affect any code that might be depending upon this string...or
// any characters that the padding would overwrite. e.g. the injected packet code.
memcpy(encrypt_packet, in_str, len);

pad_length = 16 - (len%16);

for (i = 0; i < pad_length;i++) {
memcpy(&in_str[len+i], &pad_length, sizeof(pad_length));
memcpy(&encrypt_packet[len+i], &pad_length, sizeof(pad_length));
}

return in_str;
return encrypt_packet;
}

// encrypt the data pointed to by in_str with length len
Expand Down Expand Up @@ -219,7 +227,7 @@ uint8_t aes_encrypt(__xdata unsigned char *in_str, uint8_t in_len, __xdata unsig
// 0 - CBC
// 1 - CTR
crypto_type = CRYPTO(encryption);

// We always pad the blocks with up to max 16 bytes.
// If we don't find a pile of 10 10 10....10 in the last block
// then we know that the last block was incomplete
Expand Down
6 changes: 2 additions & 4 deletions Firmware/radio/AES/aes.h
View file
Expand Up @@ -33,20 +33,18 @@
///



//=============================================================================
// Function Prototypes
//-----------------------------------------------------------------------------

extern bool aes_init(uint8_t encryption_level);

extern uint8_t aes_encrypt(__xdata unsigned char *in_str, uint8_t in_len, __xdata unsigned char *out_str, uint8_t *out_len);

extern bool aes_init(uint8_t encryption_level);

extern uint8_t aes_decrypt(__xdata unsigned char *in_str, uint8_t in_len, __xdata unsigned char *out_str, uint8_t *out_len);

extern uint8_t aes_get_encryption_level();

void aes_set_encryption_level(uint8_t encryption);


#define AES_KEY_LENGTH(_l) 8*(1 +(_l&0xf))
20 changes: 10 additions & 10 deletions Firmware/radio/packet.c
View file
Expand Up @@ -208,11 +208,11 @@ uint8_t mavlink_frame(uint8_t max_xmit, __xdata uint8_t * __pdata buf)
__xdata uint8_t len_encrypted;
#endif // INCLUDE_AES

uint8_t encryptReturn(__xdata uint8_t *buf_out, __xdata uint8_t *buf_in, uint8_t last_sent_len)
uint8_t encryptReturn(__xdata uint8_t *buf_out, __xdata uint8_t *buf_in, uint8_t buf_in_len)
{
#ifdef INCLUDE_AES
if (aes_get_encryption_level() > 0) {
if (aes_encrypt(buf_in, last_sent_len, buf_out, &len_encrypted) != 0)
if (aes_encrypt(buf_in, buf_in_len, buf_out, &len_encrypted) != 0)
{
panic("error while trying to encrypt data");
}
Expand All @@ -221,8 +221,8 @@ uint8_t encryptReturn(__xdata uint8_t *buf_out, __xdata uint8_t *buf_in, uint8_t
#endif // INCLUDE_AES

// if no encryption or not supported fall back to copy
memcpy(buf_out, buf_in, last_sent_len);
return last_sent_len;
memcpy(buf_out, buf_in, buf_in_len);
return buf_in_len;
}

// return the next packet to be sent
Expand All @@ -232,8 +232,8 @@ packet_get_next(register uint8_t max_xmit, __xdata uint8_t *buf)
register uint16_t slen;

#ifdef INCLUDE_AES
// Encryption takes 1 byte and is in factors of 16.
// 16, 32, 48 etc, lets not send anything above 32 bits back
// Encryption takes 1 byte and is in multiples of 16.
// 16, 32, 48 etc, lets not send anything above 32 bytes back
// If you change this increase the buffer in serial.c serial_write_buf()
if (aes_get_encryption_level() > 0) {
if(max_xmit <= 16) return 0;
Expand All @@ -249,13 +249,14 @@ packet_get_next(register uint8_t max_xmit, __xdata uint8_t *buf)
// sending these injected packets at full size doesn't
// seem to work well ... though I don't really know why!
if (max_xmit > 32) {
max_xmit = 32;
max_xmit = 32;
}

if (max_xmit < slen) {
// send as much as we can
last_sent_len = slen - max_xmit;
slen = encryptReturn(buf, last_sent, max_xmit);
last_sent_len = slen - max_xmit;
slen = encryptReturn(buf, last_sent, max_xmit);

memcpy(last_sent, &last_sent[max_xmit], last_sent_len);
last_sent_is_injected = true;
return slen;
Expand Down Expand Up @@ -399,7 +400,6 @@ packet_get_next(register uint8_t max_xmit, __xdata uint8_t *buf)
slen--;
}
}
// printf("ret");
return encryptReturn(buf, last_sent, last_sent_len);
}

Expand Down
39 changes: 26 additions & 13 deletions Firmware/radio/tdm.c
View file
Expand Up @@ -455,18 +455,16 @@ tdm_remote_at(void)
}

// handle an incoming at command from the remote radio
static void
//
// Return true if returning a pbuf that needs to be sent to output
// false if data is going out to the other modem
static bool
handle_at_command(__pdata uint8_t len)
{
if (len < 2 || len > AT_CMD_MAXLEN ||
pbuf[0] != (uint8_t)'R' ||
pbuf[1] != (uint8_t)'T') {
// assume its an AT command reply
register uint8_t i;
for (i=0; i<len; i++) {
putchar(pbuf[i]);
}
return;
return true;
}

// setup the command in the at_cmd buffer
Expand All @@ -485,6 +483,7 @@ handle_at_command(__pdata uint8_t len)
if (len > 0) {
packet_inject(pbuf, len);
}
return false;
}

// a stack carary to detect a stack overflow
Expand Down Expand Up @@ -582,11 +581,20 @@ tdm_serial_loop(void)
sync_tx_windows(len);
last_t = tnow;

if (trailer.command == 1) {
handle_at_command(len);
} else if (len != 0 &&
!packet_is_duplicate(len, pbuf, trailer.resend) &&
!at_mode_active) {

// Send data to console (serial buffers) if following conditions met
// If is a command and data is destined to THIS modem
// OR
// data is present, not a command, not a dup and not in AT Mode
//
// Question: Are we happy to blink Activity lights for RT command results?
if ((trailer.command == 1 && handle_at_command(len))
||
(len != 0 && trailer.command == 0 &&
!packet_is_duplicate(len, pbuf, trailer.resend) &&
!at_mode_active
))
{
// its user data - send it out
// the serial port
#ifdef INCLUDE_AES
Expand Down Expand Up @@ -734,7 +742,12 @@ tdm_serial_loop(void)
} else {
// get a packet from the serial port
len = packet_get_next(max_xmit, pbuf);
trailer.command = packet_is_injected();

if (len > 0) {
trailer.command = packet_is_injected();
} else {
trailer.command = 0;
}
#ifdef INCLUDE_AES
trailer.crc = crc16(len, pbuf);
#endif
Expand Down

0 comments on commit 2883998

Please sign in to comment.