From 859acf612b40837ab8760d3a7dd21c22b4a7b207 Mon Sep 17 00:00:00 2001 From: Scott Duensing Date: Mon, 6 Dec 2021 21:31:09 -0600 Subject: [PATCH] Start of DH key exchange code. Not working. --- client/src/main.c | 2 + shared/packet.c | 105 +++++++++++++++++++++++++++++++++++++++++----- shared/packet.h | 14 +++++++ 3 files changed, 111 insertions(+), 10 deletions(-) diff --git a/client/src/main.c b/client/src/main.c index 766af19..1bfd445 100644 --- a/client/src/main.c +++ b/client/src/main.c @@ -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 }; diff --git a/shared/packet.c b/shared/packet.c index 2b35a19..027b905 100644 --- a/shared/packet.c +++ b/shared/packet.c @@ -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; xdhMySecret[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; xdhSharedKey[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; xdhModulus[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; diff --git a/shared/packet.h b/shared/packet.h index 4c9ae7b..6d5cbf9 100644 --- a/shared/packet.h +++ b/shared/packet.h @@ -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);