From 284acc128ac18f977dfa25945149ff4f7df7c914 Mon Sep 17 00:00:00 2001 From: Travis Nickles Date: Sun, 4 Mar 2018 22:43:49 -0600 Subject: [PATCH] Added CRC32 check when obtaining gyro calibration data --- DS4Windows/DS4Library/Crc32.cs | 75 +++++++++++++++++++++++++++++- DS4Windows/DS4Library/DS4Device.cs | 44 +++++++++++++++--- 2 files changed, 112 insertions(+), 7 deletions(-) diff --git a/DS4Windows/DS4Library/Crc32.cs b/DS4Windows/DS4Library/Crc32.cs index b07ff7c..14d10ea 100644 --- a/DS4Windows/DS4Library/Crc32.cs +++ b/DS4Windows/DS4Library/Crc32.cs @@ -203,7 +203,80 @@ namespace DS4Windows return crc; } - public static uint CalculateHash(ref uint seed, ref byte[] buffer, ref int start, ref int size) + public static uint CalculateBasicHash(ref uint seed, ref byte[] buffer, int offset, int size) + { + uint crc = seed; + int i = offset; + while (size >= 16) + { + uint one = (buffer[i++] | + (uint)(buffer[i++] << 8) | + (uint)(buffer[i++] << 16) | + (uint)(buffer[i++] << 24)) ^ crc; + uint two = buffer[i++] | + (uint)(buffer[i++] << 8) | + (uint)(buffer[i++] << 16) | + (uint)(buffer[i++] << 24); + uint three = (buffer[i++] | + (uint)(buffer[i++] << 8) | + (uint)(buffer[i++] << 16) | + (uint)(buffer[i++] << 24)); + uint four = buffer[i++] | + (uint)(buffer[i++] << 8) | + (uint)(buffer[i++] << 16) | + (uint)(buffer[i++] << 24); + + crc = secondLook[15, one & 0xFF] ^ + secondLook[14, (one >> 8) & 0xFF] ^ + secondLook[13, (one >> 16) & 0xFF] ^ + secondLook[12, (one >> 24) & 0xFF] ^ + secondLook[11, two & 0xFF] ^ + secondLook[10, (two >> 8) & 0xFF] ^ + secondLook[9, (two >> 16) & 0xFF] ^ + secondLook[8, (two >> 24) & 0xFF] ^ + secondLook[7, three & 0xFF] ^ + secondLook[6, (three >> 8) & 0xFF] ^ + secondLook[5, (three >> 16) & 0xFF] ^ + secondLook[4, (three >> 24) & 0xFF] ^ + secondLook[3, four & 0xFF] ^ + secondLook[2, (four >> 8) & 0xFF] ^ + secondLook[1, (four >> 16) & 0xFF] ^ + defaultTable[(four >> 24) & 0xFF]; + + size -= 16; + } + + while (size >= 8) + { + uint one = (buffer[i++] | + (uint)(buffer[i++] << 8) | + (uint)(buffer[i++] << 16) | + (uint)(buffer[i++] << 24)) ^ crc; + uint two = buffer[i++] | + (uint)(buffer[i++] << 8) | + (uint)(buffer[i++] << 16) | + (uint)(buffer[i++] << 24); + crc = secondLook[7, one & 0xFF] ^ + secondLook[6, (one >> 8) & 0xFF] ^ + secondLook[5, (one >> 16) & 0xFF] ^ + secondLook[4, one >> 24] ^ + secondLook[3, two & 0xFF] ^ + secondLook[2, (two >> 8) & 0xFF] ^ + secondLook[1, (two >> 16) & 0xFF] ^ + defaultTable[two >> 24]; + + size -= 8; + } + + while (--size >= 0) + { + crc = (crc >> 8) ^ defaultTable[(crc & 0xFF) ^ buffer[i++]];// i++; + } + + return crc; + } + + public static uint CalculateFasterBTHash(ref uint seed, ref byte[] buffer, ref int start, ref int size) { /*uint crc = seed; for (int i = start; i < size + start; i++) diff --git a/DS4Windows/DS4Library/DS4Device.cs b/DS4Windows/DS4Library/DS4Device.cs index dcedb20..423c350 100644 --- a/DS4Windows/DS4Library/DS4Device.cs +++ b/DS4Windows/DS4Library/DS4Device.cs @@ -447,7 +447,7 @@ namespace DS4Windows touchpad = new DS4Touchpad(); sixAxis = new DS4SixAxis(); - + Crc32Algorithm.InitializeTable(DefaultPolynomial); refreshCalibration(); } @@ -468,12 +468,45 @@ namespace DS4Windows } } + const int DS4_FEATURE_REPORT_5_LEN = 41; + const int DS4_FEATURE_REPORT_5_CRC32_POS = DS4_FEATURE_REPORT_5_LEN - 4; public void refreshCalibration() { byte[] calibration = new byte[41]; calibration[0] = conType == ConnectionType.BT ? (byte)0x05 : (byte)0x02; - hDevice.readFeatureData(calibration); - sixAxis.setCalibrationData(ref calibration, conType == ConnectionType.USB); + + if (conType == ConnectionType.BT) + { + bool found = false; + for (int tries = 0; !found && tries < 5; tries++) + { + hDevice.readFeatureData(calibration); + uint recvCrc32 = calibration[DS4_FEATURE_REPORT_5_CRC32_POS] | + (uint)(calibration[DS4_FEATURE_REPORT_5_CRC32_POS + 1] << 8) | + (uint)(calibration[DS4_FEATURE_REPORT_5_CRC32_POS + 2] << 16) | + (uint)(calibration[DS4_FEATURE_REPORT_5_CRC32_POS + 3] << 24); + + uint calcCrc32 = ~Crc32Algorithm.Compute(new byte[] { 0xA3 }); + calcCrc32 = ~Crc32Algorithm.CalculateBasicHash(ref calcCrc32, ref calibration, 0, DS4_FEATURE_REPORT_5_LEN - 4); + bool validCrc = recvCrc32 == calcCrc32; + if (!validCrc && tries >= 5) + { + Log.LogToGui("Gyro Calibration Failed", true); + continue; + } + else if (validCrc) + { + found = true; + } + } + + sixAxis.setCalibrationData(ref calibration, conType == ConnectionType.USB); + } + else + { + hDevice.readFeatureData(calibration); + sixAxis.setCalibrationData(ref calibration, conType == ConnectionType.USB); + } } public void StartUpdate() @@ -691,7 +724,6 @@ namespace DS4Windows CRC32_POS_3 = BT_INPUT_REPORT_CRC32_POS + 3; int crcpos = BT_INPUT_REPORT_CRC32_POS; int crcoffset = 0; - Crc32Algorithm.InitializeTable(DefaultPolynomial); while (!exitInputThread) { @@ -725,10 +757,10 @@ namespace DS4Windows (uint)(btInputReport[CRC32_POS_2] << 16) | (uint)(btInputReport[CRC32_POS_3] << 24); - uint calcCrc32 = ~Crc32Algorithm.CalculateHash(ref HamSeed, ref btInputReport, ref crcoffset, ref crcpos); + uint calcCrc32 = ~Crc32Algorithm.CalculateFasterBTHash(ref HamSeed, ref btInputReport, ref crcoffset, ref crcpos); if (recvCrc32 != calcCrc32) { - Log.LogToGui("Check failed", true); + Log.LogToGui("Crc check failed", true); //Console.WriteLine(MacAddress.ToString() + " " + System.DateTime.UtcNow.ToString("o") + "" + // "> invalid CRC32 in BT input report: 0x" + recvCrc32.ToString("X8") + " expected: 0x" + calcCrc32.ToString("X8"));