Public_Skunkworks/kpmpgsmkii
Public_Skunkworks/kpmpgsmkii
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Start of DH key exchange code. Not working.

Browse source
This commit is contained in:
Scott Duensing 2021-12-06 21:31:09 -06:00
parent 10f5da9595
commit 859acf612b
3 changed files with 111 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
client/src/main.c
View file

@ -109,6 +109,8 @@ static void taskComDebugLoop(void *data) {
// Connected! Show icon and negotiate session.
logWrite("Connected\n");
packetEncryptionSetup(__packetThreadData);
PacketEncodeDataT encoded = { 0 };
PacketDecodeDataT decoded = { 0 };

105
shared/packet.c
View file

@ -19,12 +19,14 @@
#include "packet.h"
#include "primes.h"
static packetSender _packetSender = NULL;
static uint8_t packetCRC(char *data, uint16_t length, uint8_t startAt);
static uint8_t packetCRC(char *data, uint16_t length, uint8_t startAt);
static uint16_t packetDHCompute(uint16_t a, uint16_t m, uint16_t n);
static uint8_t packetCRC(char *data, uint16_t length, uint8_t startAt) {
@ -40,9 +42,9 @@ static uint8_t packetCRC(char *data, uint16_t length, uint8_t startAt) {
uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, char *input, uint16_t length) {
uint8_t sequence = 0;
uint16_t x = 0;
int32_t x = 0;
char c = 0;
PacketEncodeDataT nak = { 0 };
PacketEncodeDataT encoded = { 0 };
// input and inputLength are incoming raw data or NULL and 0 to continue processing already received data.
// Returns 1 on packet ready, 0 on still waiting.
@ -119,13 +121,13 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
} else {
// No! NAK it!
logWrite("Packet out of sequence! Got %d wanted %d!\n\r", sequence, threadData->lastRemoteSequence);
nak.control = PACKET_CONTROL_NAK; // Negative acknowledge.
nak.packetType = PACKET_TYPE_NONE; // Not destined for the app.
nak.channel = 0; // Channel doesn't matter for NAK.
nak.encrypt = 0; // Encryption doesn't matter for NAK.
nak.sequence = threadData->lastRemoteSequence; // The last good packet we saw.
packetEncode(threadData, &nak, NULL, 0);
packetSend(threadData, &nak);
encoded.control = PACKET_CONTROL_NAK; // Negative acknowledge.
encoded.packetType = PACKET_TYPE_NONE; // Not destined for the app.
encoded.channel = 0; // Channel doesn't matter for NAK.
encoded.encrypt = 0; // Encryption doesn't matter for NAK.
encoded.sequence = threadData->lastRemoteSequence; // The last good packet we saw.
packetEncode(threadData, &encoded, NULL, 0);
packetSend(threadData, &encoded);
continue;
}
@ -149,6 +151,44 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
}
// Fix length to remove header and checksum.
data->length -= 4;
// Is this a DH_REQUEST?
if (data->packetType == PACKET_TYPE_DH_REQUEST) {
memcpy(threadData->dhModulus, data->data, PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
memcpy(threadData->dhBase, &data->data[PACKET_ENCRYPT_KEY_SIZE * 2], PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
memcpy(threadData->dhTheirPublic, &data->data[PACKET_ENCRYPT_KEY_SIZE * 4], PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
DEL(data->data);
logWrite("Server Key: ");
for (x=0; x<PACKET_ENCRYPT_KEY_SIZE; x++) {
threadData->dhMySecret[x] = rand();
threadData->dhMyPublic[x] = packetDHCompute(threadData->dhBase[x], threadData->dhMySecret[x], threadData->dhModulus[x]);
threadData->dhSharedKey[x] = packetDHCompute(threadData->dhTheirPublic[x], threadData->dhMySecret[x], threadData->dhModulus[x]);
logWrite("%d ", threadData->dhSharedKey[x]);
}
logWrite("\n\r");
encoded.control = PACKET_CONTROL_DAT;
encoded.packetType = PACKET_TYPE_DH_RESPONSE;
encoded.channel = 0; // Doesn't matter for DH_RESPONSE.
encoded.encrypt = 0; // Must be 0 for DH_RESPONSE.
packetEncode(threadData, &encoded, (char *)threadData->dhMyPublic, PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
packetSend(threadData, &encoded);
continue;
}
// Is this a DH_RESPONSE?
if (data->packetType == PACKET_TYPE_DH_RESPONSE) {
memcpy(threadData->dhTheirPublic, data->data, PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
DEL(data->data);
logWrite("Client Key: ");
for (x=0; x<PACKET_ENCRYPT_KEY_SIZE; x++) {
threadData->dhSharedKey[x] = packetDHCompute(threadData->dhTheirPublic[x], threadData->dhMySecret[x], threadData->dhModulus[x]);
logWrite("%d ", threadData->dhSharedKey[x]);
}
logWrite("\n\r");
continue;
}
// Done!
break;
}
} else {
@ -183,6 +223,27 @@ void packetDecodeDataDestroy(PacketDecodeDataT **packet) {
}
static uint16_t packetDHCompute(uint16_t a, uint16_t m, uint16_t n) {
// See: https://www.techiedelight.com/c-program-demonstrate-diffie-hellman-algorithm/
uint16_t r = 0;
uint16_t y = 1;
while (m > 0) {
r = m % 2;
// Fast exponention.
if (r == 1) {
y = (y * a) % n;
}
a = a * a % n;
m = m / 2;
}
return y;
}
uint8_t packetEncode(PacketThreadDataT *threadData, PacketEncodeDataT *data, char *input, uint16_t length) {
uint8_t crc = 0;
uint8_t control = 0;
@ -243,6 +304,23 @@ uint8_t packetEncode(PacketThreadDataT *threadData, PacketEncodeDataT *data, cha
}
void packetEncryptionSetup(PacketThreadDataT *threadData) {
PacketEncodeDataT encoded = { 0 };
uint16_t dhData[PACKET_ENCRYPT_KEY_SIZE * 3] = { 0 };
memcpy(&dhData[0], threadData->dhModulus, PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
memcpy(&dhData[PACKET_ENCRYPT_KEY_SIZE], threadData->dhBase, PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
memcpy(&dhData[PACKET_ENCRYPT_KEY_SIZE * 2], threadData->dhMyPublic, PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
encoded.control = PACKET_CONTROL_DAT;
encoded.packetType = PACKET_TYPE_DH_REQUEST;
encoded.channel = 0; // Doesn't matter for DH_REQUEST.
encoded.encrypt = 0; // Must be 0 for DH_REQUEST.
packetEncode(threadData, &encoded, (char *)dhData, PACKET_ENCRYPT_KEY_SIZE * 3 * sizeof(uint16_t));
packetSend(threadData, &encoded);
}
void packetSend(PacketThreadDataT *threadData, PacketEncodeDataT *data) {
// Valid control type?
if (data->control != PACKET_CONTROL_BAD && data->control <= PACKET_CONTROL_COUNT) {
@ -274,6 +352,7 @@ void packetSenderRegister(packetSender sender) {
PacketThreadDataT *packetThreadDataCreate(void *senderData) {
PacketThreadDataT *data = NULL;
uint8_t x = 0;
data = (PacketThreadDataT *)malloc(sizeof(PacketThreadDataT));
if (data) {
@ -284,6 +363,12 @@ PacketThreadDataT *packetThreadDataCreate(void *senderData) {
data->decodeQueueTail = 0;
data->newPacket = 1;
data->senderData = senderData;
for (x=0; x<PACKET_ENCRYPT_KEY_SIZE; x++) {
data->dhModulus[x] = PRIMES[rand() % PRIME_COUNT];
data->dhBase[x] = (rand() < (RAND_MAX / 2) ? 2 : 5);
data->dhMySecret[x] = rand();
data->dhMyPublic[x] = packetDHCompute(data->dhBase[x], data->dhMySecret[x], data->dhModulus[x]);
}
}
return data;

14
shared/packet.h
View file

@ -36,6 +36,8 @@
* Byte X0: dddd dddd /
* Byte X1: cccc cccc - Checksum
*
* Inspired by https://github.com/BaroboRobotics/libsfp/wiki/Serial-Framing-Protocol
*
*/
@ -47,6 +49,8 @@
#define PACKET_FRAME 0x7e
#define PACKET_ENCRYPT_KEY_SIZE 32
// This enum must be 4 entries or less.
typedef enum PacketControlE {
@ -64,6 +68,8 @@ typedef enum PacketTypeE {
PACKET_TYPE_PONG,
PACKET_TYPE_SERVER_SHUTDOWN,
PACKET_TYPE_CLIENT_SHUTDOWN,
PACKET_TYPE_DH_REQUEST,
PACKET_TYPE_DH_RESPONSE,
PACKET_TYPE_COUNT
} PacketTypeT;
@ -109,14 +115,22 @@ typedef struct PacketThreadDataS {
uint8_t inEscape;
uint8_t newPacket;
void *senderData;
uint16_t dhModulus[PACKET_ENCRYPT_KEY_SIZE];
uint16_t dhBase[PACKET_ENCRYPT_KEY_SIZE];
uint16_t dhMySecret[PACKET_ENCRYPT_KEY_SIZE];
uint16_t dhMyPublic[PACKET_ENCRYPT_KEY_SIZE];
uint16_t dhTheirPublic[PACKET_ENCRYPT_KEY_SIZE];
uint16_t dhSharedKey[PACKET_ENCRYPT_KEY_SIZE];
} PacketThreadDataT;
typedef void (*packetSender)(char *data, uint32_t length, void *userData);
uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, char *input, uint16_t length);
void packetDecodeDataDestroy(PacketDecodeDataT **packet);
uint8_t packetEncode(PacketThreadDataT *threadData, PacketEncodeDataT *data, char *input, uint16_t length);
void packetEncryptionSetup(PacketThreadDataT *threadData);
void packetSend(PacketThreadDataT *threadData, PacketEncodeDataT *data);
void packetSenderRegister(packetSender sender);
PacketThreadDataT *packetThreadDataCreate(void *senderData);