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

Fixed decoding fragmented packets. DOS build now working again.

Browse source
This commit is contained in:
Scott Duensing 2021-12-08 19:18:05 -06:00
parent 9c0c7b712d
commit 8d5d5add73
7 changed files with 56 additions and 37 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
client/client.pro
View file

@ -27,6 +27,8 @@ SHARED = $$PWD/../shared
QMAKE_CFLAGS += -O0 QMAKE_CFLAGS += -O0
DEFINES *= CLIENT
DOS_HEADERS = \ DOS_HEADERS = \
src/thirdparty/serial/serial.h src/thirdparty/serial/serial.h

13
client/src/main.c
View file

@ -70,6 +70,7 @@ static void taskComDebugLoop(void *data) {
r = comOpen(__configData.serialCom - 1, 57600L, 8, 'n', 1, SER_HANDSHAKING_RTSCTS); r = comOpen(__configData.serialCom - 1, 57600L, 8, 'n', 1, SER_HANDSHAKING_RTSCTS);
if (r != SER_SUCCESS) { if (r != SER_SUCCESS) {
logWrite("Unable to open COM port! Please check settings.\n"); logWrite("Unable to open COM port! Please check settings.\n");
guiStop();
return; return;
} }
// Send a CR to clear anything in the modem. // Send a CR to clear anything in the modem.
@ -83,10 +84,11 @@ static void taskComDebugLoop(void *data) {
snprintf(buffer, 1023, "%s%c", "AT+SOCK1", 13); snprintf(buffer, 1023, "%s%c", "AT+SOCK1", 13);
comWrite(__configData.serialCom - 1, buffer, strlen(buffer)); comWrite(__configData.serialCom - 1, buffer, strlen(buffer));
// Wait roughly a second for "OK". // Wait roughly a second for "OK".
r = comWaitWithTimeout(__configData.serialCom - 1, buffer, 1023, 4, "\rOK\r"); r = comWaitWithTimeout(__configData.serialCom - 1, buffer, 1023, 4, "OK");
if (r <= 0) { if (r <= 0) {
comClose(__configData.serialCom - 1); comClose(__configData.serialCom - 1);
logWrite("Modem does not support ENET! Please check settings.\n"); logWrite("Modem does not support ENET! Please check settings.\n");
guiStop();
return; return;
} }
logWrite("Modem OK\n"); logWrite("Modem OK\n");
@ -103,6 +105,7 @@ static void taskComDebugLoop(void *data) {
if (r <= 0) { if (r <= 0) {
comClose(__configData.serialCom - 1); comClose(__configData.serialCom - 1);
logWrite("Unable to connect to server! Please check settings or try later.\n"); logWrite("Unable to connect to server! Please check settings or try later.\n");
guiStop();
return; return;
} }
@ -125,7 +128,7 @@ static void taskComDebugLoop(void *data) {
packetEncode(__packetThreadData, &encoded, NULL, 0); // Must encode each packet - no reusing encoded data. packetEncode(__packetThreadData, &encoded, NULL, 0); // Must encode each packet - no reusing encoded data.
packetSend(__packetThreadData, &encoded); packetSend(__packetThreadData, &encoded);
// Wait for PONG. // Wait for PONG.
while (1) { while (!guiHasStopped()) {
r = comRead(__configData.serialCom - 1, buffer, 1); r = comRead(__configData.serialCom - 1, buffer, 1);
if (r == 1) { if (r == 1) {
if (packetDecode(__packetThreadData, &decoded, buffer, 1)) { if (packetDecode(__packetThreadData, &decoded, buffer, 1)) {
@ -141,6 +144,9 @@ static void taskComDebugLoop(void *data) {
} }
} }
} }
if (guiHasStopped()) return;
// Send LOGIN. // Send LOGIN.
logWrite("Sending LOGIN\n"); logWrite("Sending LOGIN\n");
PacketTypeLoginT loginData; PacketTypeLoginT loginData;
@ -174,7 +180,7 @@ static void taskComDebugLoop(void *data) {
static void taskGuiEventLoop(void *data) { static void taskGuiEventLoop(void *data) {
MouseT *mouse = NULL; MouseT *mouse = NULL;
ImageT *pointer = NULL; ImageT *pointer = NULL;
ColorT alpha; ColorT alpha = { 0 };
(void)data; (void)data;
@ -187,6 +193,7 @@ static void taskGuiEventLoop(void *data) {
if (keyHit()) { if (keyHit()) {
guiKeyboardProcess(keyASCIIGet(), keyExtendedGet(), keyScanCodeGet(), keyShiftGet(), keyControlGet(), keyAltGet()); guiKeyboardProcess(keyASCIIGet(), keyExtendedGet(), keyScanCodeGet(), keyShiftGet(), keyControlGet(), keyAltGet());
//logWrite("Key '%d' Extended '%d' Scancode '%d' Shift '%d' Control '%d' Alt '%d'\n", keyASCIIGet(), keyExtended(), keyScanCode(), keyShift(), keyControl(), keyAlt()); //logWrite("Key '%d' Extended '%d' Scancode '%d' Shift '%d' Control '%d' Alt '%d'\n", keyASCIIGet(), keyExtended(), keyScanCode(), keyShift(), keyControl(), keyAlt());
if (keyASCIIGet() == 27) guiStop();
} }
guiMouseProcess(mouse); guiMouseProcess(mouse);
guiComposite(); guiComposite();

3
client/src/system/comport.c
View file

@ -72,11 +72,12 @@ int comWaitWithTimeout(int com, char *buffer, int len, int quarterSeconds, char
// Out of buffer. // Out of buffer.
break; break;
} }
} else {
taskYield();
} }
if (__timerQuarterSecondTick) { if (__timerQuarterSecondTick) {
quarterTicks++; quarterTicks++;
} }
taskYield();
} }
if (count == (int)strlen(expecting)) { if (count == (int)strlen(expecting)) {

2
server/server.pro
View file

@ -26,6 +26,8 @@ CONFIG += \
DESTDIR = $$OUT_PWD/bin DESTDIR = $$OUT_PWD/bin
SHARED = $$PWD/../shared SHARED = $$PWD/../shared
DEFINES *= SERVER
INCLUDEPATH += \ INCLUDEPATH += \
/usr/include/mariadb \ /usr/include/mariadb \
/usr/include/mariadb/mysql \ /usr/include/mariadb/mysql \

8
server/src/client.c
View file

@ -104,6 +104,14 @@ void clientQueuePacket(ClientThreadT *client, uint8_t *data, uint32_t length) {
packet->length = length; packet->length = length;
} }
//consoleMessageQueue("%ld: Channel %d Bytes in %d\n", client->threadIndex, length);
/*
logWrite("Bytes %d\n\r", length);
for (size_t x=0; x<length; x++) {
logWrite("[%x] '%c'\n\r", data[x], data[x]);
}
*/
pthread_mutex_lock(&client->packetQueueMutex); pthread_mutex_lock(&client->packetQueueMutex);
arrput(client->packetQueue, packet); arrput(client->packetQueue, packet);
pthread_mutex_unlock(&client->packetQueueMutex); pthread_mutex_unlock(&client->packetQueueMutex);

62
shared/packet.c
View file

@ -63,7 +63,7 @@ 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 packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *decodeData, char *input, uint16_t inputLength) {
uint8_t sequence = 0; uint8_t sequence = 0;
uint16_t unpadded = 0; uint16_t unpadded = 0;
int32_t x = 0; int32_t x = 0;
@ -74,12 +74,10 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
// Returns 1 on packet ready, 0 on still waiting. // Returns 1 on packet ready, 0 on still waiting.
// Is there input data to add to the queue? // Is there input data to add to the queue?
if (input != NULL && length > 0) { if (input != NULL && inputLength > 0) {
for (x=0; x<length; x++) { for (x=0; x<inputLength; x++) {
threadData->decodeQueue[threadData->decodeQueueHead++] = input[x]; threadData->decodeQueue[threadData->decodeQueueHead++] = input[x];
if (threadData->decodeQueueHead >= PACKET_INPUT_QUEUE_SIZE) { if (threadData->decodeQueueHead >= PACKET_INPUT_QUEUE_SIZE) threadData->decodeQueueHead = 0;
threadData->decodeQueueHead = 0;
}
} }
} }
@ -89,15 +87,13 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
// Get next byte. // Get next byte.
c = threadData->decodeQueue[threadData->decodeQueueTail++]; c = threadData->decodeQueue[threadData->decodeQueueTail++];
if (threadData->decodeQueueTail >= PACKET_INPUT_QUEUE_SIZE) { if (threadData->decodeQueueTail >= PACKET_INPUT_QUEUE_SIZE) threadData->decodeQueueTail = 0;
threadData->decodeQueueTail = 0;
}
// New packet? // New packet?
if (threadData->newPacket) { if (threadData->newPacket) {
threadData->newPacket = 0; threadData->newPacket = 0;
threadData->inEscape = 0; threadData->inEscape = 0;
data->length = 0; threadData->length = 0;
} }
// Are we escaped? // Are we escaped?
@ -108,7 +104,7 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
threadData->newPacket = 1; threadData->newPacket = 1;
// Check CRC. // Check CRC.
if ((uint8_t)threadData->decodeBuffer[data->length - 1] != packetCRC(threadData->decodeBuffer, data->length - 1, 0)) continue; if ((uint8_t)threadData->decodeBuffer[threadData->length - 1] != packetCRC(threadData->decodeBuffer, threadData->length - 1, 0)) continue;
// Get sequence value. // Get sequence value.
sequence = ((uint8_t)threadData->decodeBuffer[0]) & 0x1f; sequence = ((uint8_t)threadData->decodeBuffer[0]) & 0x1f;
@ -156,36 +152,38 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
} }
// Fill decoded data fields. // Fill decoded data fields.
data->packetType = threadData->decodeBuffer[1]; decodeData->packetType = threadData->decodeBuffer[1];
data->channel = threadData->decodeBuffer[2]; decodeData->channel = threadData->decodeBuffer[2];
// AES Decryption. // AES Decryption.
if (threadData->decodeBuffer[0] & 32) { if (threadData->decodeBuffer[0] & 32) {
AES_ctx_set_iv(threadData->aesContext, (uint8_t *)threadData->dhModulus); AES_ctx_set_iv(threadData->aesContext, (uint8_t *)threadData->dhModulus);
AES_CBC_decrypt_buffer(threadData->aesContext, (uint8_t *)&threadData->decodeBuffer[3], data->length - 4); AES_CBC_decrypt_buffer(threadData->aesContext, (uint8_t *)&threadData->decodeBuffer[3], threadData->length - 4);
unpadded = pkcs7_padding_data_length((uint8_t *)&threadData->decodeBuffer[3], data->length - 4, 16); unpadded = pkcs7_padding_data_length((uint8_t *)&threadData->decodeBuffer[3], threadData->length - 4, 16);
// Fix length. // Fix length.
data->length = unpadded + 4; threadData->length = unpadded + 4;
} }
// Copy packet data to new buffer, if any. // Copy packet data to new buffer, if any.
if (data->length - 4 > 0) { if (threadData->length - 4 > 0) {
data->data = (char *)malloc(data->length - 4); // 4 for 3 byte header and 1 byte CRC. decodeData->data = (char *)malloc(threadData->length - 4); // 4 for 3 byte header and 1 byte CRC.
if (!data) continue; if (!decodeData) continue;
memcpy(data->data, &threadData->decodeBuffer[3], data->length - 4); // Skip header and CRC. memcpy(decodeData->data, &threadData->decodeBuffer[3], threadData->length - 4); // Skip header and CRC.
decodeData->length = threadData->length - 4;
} else { } else {
// No payload. // No payload.
data->data = NULL; decodeData->data = NULL;
decodeData->length = 0;
} }
// Fix length to remove header and checksum. // Fix length to remove header and checksum.
data->length -= 4; threadData->length -= 4;
// Is this a DH_REQUEST? // Is this a DH_REQUEST?
if (data->packetType == PACKET_TYPE_DH_REQUEST) { if (decodeData->packetType == PACKET_TYPE_DH_REQUEST) {
memcpy(threadData->dhModulus, data->data, PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t)); memcpy(threadData->dhModulus, decodeData->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->dhBase, &decodeData->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)); memcpy(threadData->dhTheirPublic, &decodeData->data[PACKET_ENCRYPT_KEY_SIZE * 4], PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
DEL(data->data); DEL(decodeData->data);
for (x=0; x<PACKET_ENCRYPT_KEY_SIZE; x++) { for (x=0; x<PACKET_ENCRYPT_KEY_SIZE; x++) {
threadData->dhMySecret[x] = rand(); threadData->dhMySecret[x] = rand();
threadData->dhMyPublic[x] = packetDHCompute(threadData->dhBase[x], threadData->dhMySecret[x], threadData->dhModulus[x]); threadData->dhMyPublic[x] = packetDHCompute(threadData->dhBase[x], threadData->dhMySecret[x], threadData->dhModulus[x]);
@ -202,9 +200,9 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
} }
// Is this a DH_RESPONSE? // Is this a DH_RESPONSE?
if (data->packetType == PACKET_TYPE_DH_RESPONSE) { if (decodeData->packetType == PACKET_TYPE_DH_RESPONSE) {
memcpy(threadData->dhTheirPublic, data->data, PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t)); memcpy(threadData->dhTheirPublic, decodeData->data, PACKET_ENCRYPT_KEY_SIZE * sizeof(uint16_t));
DEL(data->data); DEL(decodeData->data);
for (x=0; x<PACKET_ENCRYPT_KEY_SIZE; x++) { for (x=0; x<PACKET_ENCRYPT_KEY_SIZE; x++) {
threadData->dhSharedKey[x] = packetDHCompute(threadData->dhTheirPublic[x], threadData->dhMySecret[x], threadData->dhModulus[x]); threadData->dhSharedKey[x] = packetDHCompute(threadData->dhTheirPublic[x], threadData->dhMySecret[x], threadData->dhModulus[x]);
} }
@ -223,13 +221,13 @@ uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, cha
continue; continue;
} }
// Did we overflow? // Did we overflow?
if (data->length >= PACKET_MAX) { if (threadData->length >= PACKET_MAX) {
// Yup. Dump it. // Yup. Dump it.
threadData->newPacket = 1; threadData->newPacket = 1;
continue; continue;
} }
// Add byte to packet. // Add byte to packet.
threadData->decodeBuffer[data->length++] = c; threadData->decodeBuffer[threadData->length++] = c;
} }
} }

3
shared/packet.h
View file

@ -101,6 +101,7 @@ typedef struct PacketThreadDataS {
uint16_t decodeQueueTail; uint16_t decodeQueueTail;
uint8_t inEscape; uint8_t inEscape;
uint8_t newPacket; uint8_t newPacket;
uint16_t length;
void *senderData; void *senderData;
uint16_t dhModulus[PACKET_ENCRYPT_KEY_SIZE]; uint16_t dhModulus[PACKET_ENCRYPT_KEY_SIZE];
uint16_t dhBase[PACKET_ENCRYPT_KEY_SIZE]; uint16_t dhBase[PACKET_ENCRYPT_KEY_SIZE];
@ -116,7 +117,7 @@ typedef struct PacketThreadDataS {
typedef void (*packetSender)(char *data, uint32_t length, void *userData); typedef void (*packetSender)(char *data, uint32_t length, void *userData);
uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *data, char *input, uint16_t length); uint8_t packetDecode(PacketThreadDataT *threadData, PacketDecodeDataT *decodeData, char *input, uint16_t inputLength);
void packetDecodeDataDestroy(PacketDecodeDataT **packet); void packetDecodeDataDestroy(PacketDecodeDataT **packet);
uint8_t packetEncode(PacketThreadDataT *threadData, PacketEncodeDataT *data, char *input, uint16_t length); uint8_t packetEncode(PacketThreadDataT *threadData, PacketEncodeDataT *data, char *input, uint16_t length);
void packetEncryptionSetup(PacketThreadDataT *threadData); void packetEncryptionSetup(PacketThreadDataT *threadData);