C
#include <WiFiUdp.h>
#include "GT7UDPParser.h"
#include "Salsa20.h"
#include <string>
#include <array>
constexpr unsigned int localPort = 33740;
constexpr unsigned int remotePort = 33739;
constexpr int PACKET_A_SIZE = 296;
constexpr int PACKET_B_SIZE = 316;
constexpr int PACKET_TILDA_SIZE = 344;
constexpr int PACKET_C_SIZE = 368;
const std::string Key = "Simulator Interface Packet GT7 ver 0.0";
union IntToBytes {
uint32_t integer;
uint8_t bytes[4];
};
std::array<uint8_t, 32> GT7_UDP_Parser::getAsciiBytes(const std::string& inputString) {
std::array<uint8_t, 32> asciiBytes = {};
for (size_t i = 0; i < inputString.size() && i < asciiBytes.size(); ++i) {
asciiBytes[i] = static_cast<uint8_t>(inputString[i]);
}
return asciiBytes;
}
void GT7_UDP_Parser::begin(const IPAddress playstationIP, const char packetVersion) {
Udp.begin(localPort);
remoteIP = playstationIP;
if ((packetVersion == 'A') || (packetVersion == 'B') || (packetVersion == '~') || (packetVersion == 'C')) {
heartbeatMsg = packetVersion;
} else {
heartbeatMsg = 'A';
}
dKey = getAsciiBytes(Key);
}
void GT7_UDP_Parser::sendHeartbeat(void) {
Udp.beginPacket(remoteIP, remotePort);
Udp.write(heartbeatMsg);
Udp.endPacket();
}
uint8_t GT7_UDP_Parser::getCurrentGearFromByte(void) {
return packet.packetContent.gears & 0b00001111; // Extract the lower 4 bits for gears
}
// Function to extract suggested gear from the byte
uint8_t GT7_UDP_Parser::getSuggestedGearFromByte(void) {
return packet.packetContent.gears >> 4; // Shift right by 4 bits to get the upper 4 bits for suggested gear
}
uint8_t GT7_UDP_Parser::getPowertrainType(void) {
if (static_cast<uint8_t>(packet.packetContent.fuelCapacity) > 10) {
return 0;
} else {
switch(static_cast<uint8_t>(packet.packetContent.fuelCapacity)) {
case 0: return 1;
break;
case 5: return 2;
break;
default: return 255;
}
}
}
float GT7_UDP_Parser::getTyreSpeed(int index) {
if (index >= 0 && index < 4) {
return abs(3.6f * packet.packetContent.tyreRadius[index] * packet.packetContent.wheelRPS[index]);
} else return 0.0f;
}
float GT7_UDP_Parser::getTyreSlipRatio(int index) {
float carSpeed = (packet.packetContent.speed * 3.6);
float tyreSpeed = getTyreSpeed(index);
if (carSpeed != 0.0f) {
return tyreSpeed / carSpeed;
} else return 0.0f;
}
uint8_t GT7_UDP_Parser::getFlag(int index) {
SimulatorFlags flags = packet.packetContent.flags;
if (index < 0 || index > 12) {
return 0;
}
if (index == 0) {
return (static_cast<int16_t>(flags) == 0) ? 1 : 0;
} else {
return (static_cast<int16_t>(flags) & (1 << (index - 1))) ? 1 : 0;
}
}
Packet GT7_UDP_Parser::readData() {
uint8_t recvBuffer[sizeof(packet.packetContent)] = {};
const int packetSize = Udp.parsePacket();
// 沒有收到 UDP 封包,不要繼續讀取或解密
if (packetSize <= 0)
{
return packet;
}
// 避免超過接收 buffer
if (packetSize > static_cast<int>(sizeof(recvBuffer)))
{
// 丟棄過大的封包
while (Udp.available())
{
Udp.read();
}
return packet;
}
const int byteStream = Udp.read(
recvBuffer,
sizeof(recvBuffer)
);
// UDP read 失敗或沒有實際資料
if (byteStream <= 0)
{
return packet;
}
if (byteStream == PACKET_A_SIZE) {
detectedPacketVersion = 'A';
} else if (byteStream == PACKET_B_SIZE) {
detectedPacketVersion = 'B';
} else if (byteStream == PACKET_TILDA_SIZE) {
detectedPacketVersion = '~';
} else if (byteStream == PACKET_C_SIZE) {
detectedPacketVersion = 'C';
} else {
return packet;
}
int iv1 = *reinterpret_cast<int*>(&recvBuffer[0x40]); // Seed IV is always located there
switch (detectedPacketVersion) {
case 'A': iv2 = iv1 ^ 0xDEADBEAF;
break;
case 'B': iv2 = iv1 ^ 0xDEADBEEF;
break;
case '~': iv2 = iv1 ^ 0x55FABB4F;
break;
case 'C': iv2 = iv1 ^ 0xDEADBEEF;
break;
default: iv2 = iv1;
break;
}
IntToBytes iv1Bytes, iv2Bytes;
iv1Bytes.integer = iv1;
iv2Bytes.integer = iv2;
uint8_t iv[8] = {
iv2Bytes.bytes[0], iv2Bytes.bytes[1], iv2Bytes.bytes[2], iv2Bytes.bytes[3],
iv1Bytes.bytes[0], iv1Bytes.bytes[1], iv1Bytes.bytes[2], iv1Bytes.bytes[3]
};
ucstk::Salsa20 salsa20(dKey.data());
salsa20.setIv(iv);
std::array<uint8_t, sizeof(packet.packetContent)> decryptedData;
salsa20.processBytes((recvBuffer), decryptedData.data(), byteStream);
uint32_t magic = 0;
memcpy(&magic, decryptedData.data(), sizeof(magic));
if (magic != 0x47375330) return packet;
// Zero fields absent in shorter packet variants instead of retaining old data.
memset(&packet.packetContent, 0, sizeof(packet.packetContent));
memcpy(&packet.packetContent, decryptedData.data(), byteStream);
return packet;
}