From 674acd84317fbb7bcde4da1be608826dee7a9e5b Mon Sep 17 00:00:00 2001 From: Jake Date: Thu, 18 May 2017 22:06:31 -0500 Subject: [PATCH] ds4: use controller calibration values for accel/gyro --- Utilities/CRC.h | 1702 +++++++++++++++++++++++++++++++++ rpcs3/DS4PadHandler.cpp | 346 +++++-- rpcs3/DS4PadHandler.h | 55 +- rpcs3/emucore.vcxproj | 1 + rpcs3/emucore.vcxproj.filters | 3 + 5 files changed, 2000 insertions(+), 107 deletions(-) create mode 100644 Utilities/CRC.h diff --git a/Utilities/CRC.h b/Utilities/CRC.h new file mode 100644 index 0000000000..9faf9174ef --- /dev/null +++ b/Utilities/CRC.h @@ -0,0 +1,1702 @@ +/** +@file CRC.h +@author Daniel Bahr +@version 0.2.0.6 +@copyright +@parblock +CRC++ +Copyright (c) 2016, Daniel Bahr +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this +list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. + +* Neither the name of CRC++ nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +@endparblock +*/ + +/* +CRC++ can be configured by setting various #defines before #including this header file: + +#define crcpp_uint8 - Specifies the type used to store CRCs that have a width of 8 bits or less. +This type is not used in CRC calculations. Defaults to ::std::uint8_t. +#define crcpp_uint16 - Specifies the type used to store CRCs that have a width between 9 and 16 bits (inclusive). +This type is not used in CRC calculations. Defaults to ::std::uint16_t. +#define crcpp_uint32 - Specifies the type used to store CRCs that have a width between 17 and 32 bits (inclusive). +This type is not used in CRC calculations. Defaults to ::std::uint32_t. +#define crcpp_uint64 - Specifies the type used to store CRCs that have a width between 33 and 64 bits (inclusive). +This type is not used in CRC calculations. Defaults to ::std::uint64_t. +#define crcpp_size - This type is used for loop iteration and function signatures only. Defaults to ::std::size_t. +#define CRCPP_USE_NAMESPACE - Define to place all CRC++ code within the ::CRCPP namespace. +#define CRCPP_BRANCHLESS - Define to enable a branchless CRC implementation. The branchless implementation uses a single integer +multiplication in the bit-by-bit calculation instead of a small conditional. The branchless implementation +may be faster on processor architectures which support single-instruction integer multiplication. +#define CRCPP_USE_CPP11 - Define to enables C++11 features (move semantics, constexpr, static_assert, etc.). +#define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. +*/ + +#define CRCPP_USE_NAMESPACE +#define CRCPP_BRANCHLESS +#define CRCPP_USE_CPP11 + +#ifndef CRCPP_CRC_H_ +#define CRCPP_CRC_H_ + +#include // Includes CHAR_BIT +#ifdef CRCPP_USE_CPP11 +#include // Includes ::std::size_t +#include // Includes ::std::uint8_t, ::std::uint16_t, ::std::uint32_t, ::std::uint64_t +#else +#include // Includes size_t +#include // Includes uint8_t, uint16_t, uint32_t, uint64_t +#endif +#include // Includes ::std::numeric_limits +#include // Includes ::std::move + +#ifndef crcpp_uint8 +# ifdef CRCPP_USE_CPP11 +/// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint8 ::std::uint8_t +# else +/// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint8 uint8_t +# endif +#endif + +#ifndef crcpp_uint16 +# ifdef CRCPP_USE_CPP11 +/// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint16 ::std::uint16_t +# else +/// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint16 uint16_t +# endif +#endif + +#ifndef crcpp_uint32 +# ifdef CRCPP_USE_CPP11 +/// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint32 ::std::uint32_t +# else +/// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint32 uint32_t +# endif +#endif + +#ifndef crcpp_uint64 +# ifdef CRCPP_USE_CPP11 +/// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint64 ::std::uint64_t +# else +/// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. +# define crcpp_uint64 uint64_t +# endif +#endif + +#ifndef crcpp_size +# ifdef CRCPP_USE_CPP11 +/// @brief Unsigned size definition, used for specifying data sizes. +# define crcpp_size ::std::size_t +# else +/// @brief Unsigned size definition, used for specifying data sizes. +# define crcpp_size size_t +# endif +#endif + +#ifdef CRCPP_USE_CPP11 +/// @brief Compile-time expression definition. +# define crcpp_constexpr constexpr +#else +/// @brief Compile-time expression definition. +# define crcpp_constexpr const +#endif + +#ifdef CRCPP_USE_NAMESPACE +namespace CRCPP +{ +#endif + + /** + @brief Static class for computing CRCs. + @note This class supports computation of full and multi-part CRCs, using a bit-by-bit algorithm or a + byte-by-byte lookup table. The CRCs are calculated using as many optimizations as is reasonable. + If compiling with C++11, the constexpr keyword is used liberally so that many calculations are + performed at compile-time instead of at runtime. + */ + class CRC + { + public: + // Forward declaration + template + struct Table; + + /** + @brief CRC parameters. + */ + template + struct Parameters + { + CRCType polynomial; ///< CRC polynomial + CRCType initialValue; ///< Initial CRC value + CRCType finalXOR; ///< Value to XOR with the final CRC + bool reflectInput; ///< true to reflect all input bytes + bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) + + Table MakeTable() const; + }; + + /** + @brief CRC lookup table. After construction, the CRC parameters are fixed. + @note A CRC table can be used for multiple CRC calculations. + */ + template + struct Table + { + // Constructors are intentionally NOT marked explicit. + Table(const Parameters & parameters); + +#ifdef CRCPP_USE_CPP11 + Table(Parameters && parameters); +#endif + + const Parameters & GetParameters() const; + + const CRCType * GetTable() const; + + CRCType operator[](unsigned char index) const; + + private: + void InitTable(); + + Parameters parameters; ///< CRC parameters used to construct the table + CRCType table[1 << CHAR_BIT]; ///< CRC lookup table + }; + + // The number of bits in CRCType must be at least as large as CRCWidth. + // CRCType must be an unsigned integer type or a custom type with operator overloads. + template + static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters); + + template + static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); + + template + static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable); + + template + static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); + + // Common CRCs up to 64 bits. + // Note: Check values are the computed CRCs when given an ASCII input of "123456789" (without null terminator) +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters< crcpp_uint8, 4> & CRC_4_ITU(); + static const Parameters< crcpp_uint8, 5> & CRC_5_EPC(); + static const Parameters< crcpp_uint8, 5> & CRC_5_ITU(); + static const Parameters< crcpp_uint8, 5> & CRC_5_USB(); + static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000A(); + static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000B(); + static const Parameters< crcpp_uint8, 6> & CRC_6_ITU(); + static const Parameters< crcpp_uint8, 7> & CRC_7(); +#endif + static const Parameters< crcpp_uint8, 8> & CRC_8(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters< crcpp_uint8, 8> & CRC_8_EBU(); + static const Parameters< crcpp_uint8, 8> & CRC_8_MAXIM(); + static const Parameters< crcpp_uint8, 8> & CRC_8_WCDMA(); + static const Parameters & CRC_10(); + static const Parameters & CRC_10_CDMA2000(); + static const Parameters & CRC_11(); + static const Parameters & CRC_12_CDMA2000(); + static const Parameters & CRC_12_DECT(); + static const Parameters & CRC_12_UMTS(); + static const Parameters & CRC_13_BBC(); + static const Parameters & CRC_15(); + static const Parameters & CRC_15_MPT1327(); +#endif + static const Parameters & CRC_16_ARC(); + static const Parameters & CRC_16_BUYPASS(); + static const Parameters & CRC_16_CCITTFALSE(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_16_CDMA2000(); + static const Parameters & CRC_16_DECTR(); + static const Parameters & CRC_16_DECTX(); + static const Parameters & CRC_16_DNP(); +#endif + static const Parameters & CRC_16_GENIBUS(); + static const Parameters & CRC_16_KERMIT(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_16_MAXIM(); + static const Parameters & CRC_16_MODBUS(); + static const Parameters & CRC_16_T10DIF(); + static const Parameters & CRC_16_USB(); +#endif + static const Parameters & CRC_16_X25(); + static const Parameters & CRC_16_XMODEM(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_17_CAN(); + static const Parameters & CRC_21_CAN(); + static const Parameters & CRC_24(); + static const Parameters & CRC_24_FLEXRAYA(); + static const Parameters & CRC_24_FLEXRAYB(); + static const Parameters & CRC_30(); +#endif + static const Parameters & CRC_32(); + static const Parameters & CRC_32_BZIP2(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_32_C(); +#endif + static const Parameters & CRC_32_MPEG2(); + static const Parameters & CRC_32_POSIX(); +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + static const Parameters & CRC_32_Q(); + static const Parameters & CRC_40_GSM(); + static const Parameters & CRC_64(); +#endif + +#ifdef CRCPP_USE_CPP11 + CRC() = delete; + CRC(const CRC & other) = delete; + CRC & operator=(const CRC & other) = delete; + CRC(CRC && other) = delete; + CRC & operator=(CRC && other) = delete; +#endif + + private: +#ifndef CRCPP_USE_CPP11 + CRC(); + CRC(const CRC & other); + CRC & operator=(const CRC & other); +#endif + + template + static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); + + template + static CRCType Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); + + template + static CRCType UndoFinalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); + + template + static CRCType CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder); + + template + static CRCType CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder); + + template + static crcpp_constexpr IntegerType BoundedConstexprValue(IntegerType x); + }; + + /** + @brief Returns a CRC lookup table construct using these CRC parameters. + @note This function primarily exists to allow use of the auto keyword instead of instantiating + a table directly, since template parameters are not inferred in constructors. + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC lookup table + */ + template + inline CRC::Table CRC::Parameters::MakeTable() const + { + // This should take advantage of RVO and optimize out the copy. + return CRC::Table(*this); + } + + /** + @brief Constructs a CRC table from a set of CRC parameters + @param[in] parameters CRC parameters + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + */ + template + inline CRC::Table::Table(const Parameters & parameters) : + parameters(parameters) + { + InitTable(); + } + +#ifdef CRCPP_USE_CPP11 + /** + @brief Constructs a CRC table from a set of CRC parameters + @param[in] parameters CRC parameters + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + */ + template + inline CRC::Table::Table(Parameters && parameters) : + parameters(::std::move(parameters)) + { + InitTable(); + } +#endif + + /** + @brief Gets the CRC parameters used to construct the CRC table + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC parameters + */ + template + inline const CRC::Parameters & CRC::Table::GetParameters() const + { + return parameters; + } + + /** + @brief Gets the CRC table + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC table + */ + template + inline const CRCType * CRC::Table::GetTable() const + { + return table; + } + + /** + @brief Gets an entry in the CRC table + @param[in] index Index into the CRC table + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC table entry + */ + template + inline CRCType CRC::Table::operator[](unsigned char index) const + { + return table[index]; + } + + /** + @brief Initializes a CRC table. + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + */ + template + inline void CRC::Table::InitTable() + { + // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK((CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1))); + + static crcpp_constexpr CRCType SHIFT(CRC::BoundedConstexprValue(CHAR_BIT - CRCWidth)); + + CRCType crc; + unsigned char byte = 0; + + // Loop over each dividend (each possible number storable in an unsigned char) + do + { + crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(0)); + + // This mask might not be necessary; all unit tests pass with this line commented out, + // but that might just be a coincidence based on the CRC parameters used for testing. + // In any case, this is harmless to leave in and only adds a single machine instruction per loop iteration. + crc &= BIT_MASK; + + if (!parameters.reflectInput && CRCWidth < CHAR_BIT) + { + // Undo the special operation at the end of the CalculateRemainder() + // function for non-reflected CRCs < CHAR_BIT. + crc <<= SHIFT; + } + + table[byte] = crc; + } while (++byte); + } + + /** + @brief Computes a CRC. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data + @param[in] parameters CRC parameters + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC + */ + template + inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters) + { + CRCType remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + } + /** + @brief Appends additional data to a previous CRC calculation. + @note This function can be used to compute multi-part CRCs. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data + @param[in] parameters CRC parameters + @param[in] crc CRC from a previous calculation + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC + */ + template + inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) + { + CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + + remainder = CalculateRemainder(data, size, parameters, remainder); + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + } + + /** + @brief Computes a CRC via a lookup table. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data + @param[in] lookupTable CRC lookup table + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC + */ + template + inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable) + { + const Parameters & parameters = lookupTable.GetParameters(); + + CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + } + + /** + @brief Appends additional data to a previous CRC calculation using a lookup table. + @note This function can be used to compute multi-part CRCs. + @param[in] data Data over which CRC will be computed + @param[in] size Size of the data + @param[in] lookupTable CRC lookup table + @param[in] crc CRC from a previous calculation + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC + */ + template + inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) + { + const Parameters & parameters = lookupTable.GetParameters(); + + CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + + remainder = CalculateRemainder(data, size, lookupTable, remainder); + + // No need to mask the remainder here; the mask will be applied in the Finalize() function. + + return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + } + + /** + @brief Reflects (i.e. reverses the bits within) an integer value. + @param[in] value Value to reflect + @param[in] numBits Number of bits in the integer which will be reflected + @tparam IntegerType Integer type of the value being reflected + @return Reflected value + */ + template + inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) + { + IntegerType reversedValue(0); + + for (crcpp_uint16 i = 0; i < numBits; ++i) + { + reversedValue = (reversedValue << 1) | (value & 1); + value >>= 1; + } + + return reversedValue; + } + + /** + @brief Computes the final reflection and XOR of a CRC remainder. + @param[in] remainder CRC remainder to reflect and XOR + @param[in] finalXOR Final value to XOR with the remainder + @param[in] reflectOutput true to reflect each byte of the remainder before the XOR + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return Final CRC + */ + template + inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput) + { + // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); + + if (reflectOutput) + { + remainder = Reflect(remainder, CRCWidth); + } + + return (remainder ^ finalXOR) & BIT_MASK; + } + + /** + @brief Undoes the process of computing the final reflection and XOR of a CRC remainder. + @note This function allows for computation of multi-part CRCs + @note Calling UndoFinalize() followed by Finalize() (or vice versa) will always return the original remainder value: + + CRCType x = ...; + CRCType y = Finalize(x, finalXOR, reflectOutput); + CRCType z = UndoFinalize(y, finalXOR, reflectOutput); + assert(x == z); + + @param[in] crc Reflected and XORed CRC + @param[in] finalXOR Final value XORed with the remainder + @param[in] reflectOutput true if the remainder is to be reflected + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return Un-finalized CRC remainder + */ + template + inline CRCType CRC::UndoFinalize(CRCType crc, CRCType finalXOR, bool reflectOutput) + { + // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); + + crc = (crc & BIT_MASK) ^ finalXOR; + + if (reflectOutput) + { + crc = Reflect(crc, CRCWidth); + } + + return crc; + } + + /** + @brief Computes a CRC remainder. + @param[in] data Data over which the remainder will be computed + @param[in] size Size of the data + @param[in] parameters CRC parameters + @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC remainder + */ + template + inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder) + { +#ifdef CRCPP_USE_CPP11 + // This static_assert is put here because this function will always be compiled in no matter what + // the template parameters are and whether or not a table lookup or bit-by-bit algorithm is used. + static_assert(::std::numeric_limits::digits >= CRCWidth, "CRCType is too small to contain a CRC of width CRCWidth."); +#else + // Catching this compile-time error is very important. Sadly, the compiler error will be very cryptic, but it's + // better than nothing. + enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; +#endif + + const unsigned char * current = reinterpret_cast(data); + + // Slightly different implementations based on the parameters. The current implementations try to eliminate as much + // computation from the inner loop (looping over each bit) as possible. + if (parameters.reflectInput) + { + CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); + while (size--) + { + remainder ^= *current++; + + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) + { +#ifdef CRCPP_BRANCHLESS + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & 1) + // remainder = (remainder >> 1) ^ polynomial; + // else + // remainder >>= 1; + remainder = (remainder >> 1) ^ ((remainder & 1) * polynomial); +#else + remainder = (remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> 1); +#endif + } + } + } + else if (CRCWidth >= CHAR_BIT) + { + static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); +#ifndef CRCPP_BRANCHLESS + static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); +#endif + static crcpp_constexpr CRCType SHIFT(BoundedConstexprValue(CRCWidth - CHAR_BIT)); + + while (size--) + { + remainder ^= (static_cast(*current++) << SHIFT); + + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) + { +#ifdef CRCPP_BRANCHLESS + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & CRC_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ parameters.polynomial; + // else + // remainder <<= 1; + remainder = (remainder << 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial); +#else + remainder = (remainder & CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1); +#endif + } + } + } + else + { + static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); +#ifndef CRCPP_BRANCHLESS + static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); +#endif + static crcpp_constexpr CRCType SHIFT(BoundedConstexprValue(CHAR_BIT - CRCWidth)); + + CRCType polynomial = parameters.polynomial << SHIFT; + remainder <<= SHIFT; + + while (size--) + { + remainder ^= *current++; + + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) + { +#ifdef CRCPP_BRANCHLESS + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ polynomial; + // else + // remainder <<= 1; + remainder = (remainder << 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial); +#else + remainder = (remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1); +#endif + } + } + + remainder >>= SHIFT; + } + + return remainder; + } + + /** + @brief Computes a CRC remainder using lookup table. + @param[in] data Data over which the remainder will be computed + @param[in] size Size of the data + @param[in] lookupTable CRC lookup table + @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return CRC remainder + */ + template + inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder) + { + const unsigned char * current = reinterpret_cast(data); + + if (lookupTable.GetParameters().reflectInput) + { + while (size--) + { +#if defined(WIN32) || defined(_WIN32) || defined(WINCE) + // Disable warning about data loss when doing (remainder >> CHAR_BIT) when + // remainder is one byte long. The algorithm is still correct in this case, + // though it's possible that one additional machine instruction will be executed. +# pragma warning (push) +# pragma warning (disable : 4333) +#endif + remainder = (remainder >> CHAR_BIT) ^ lookupTable[static_cast(remainder ^ *current++)]; +#if defined(WIN32) || defined(_WIN32) || defined(WINCE) +# pragma warning (pop) +#endif + } + } + else if (CRCWidth >= CHAR_BIT) + { + static crcpp_constexpr CRCType SHIFT(BoundedConstexprValue(CRCWidth - CHAR_BIT)); + + while (size--) + { + remainder = (remainder << CHAR_BIT) ^ lookupTable[static_cast((remainder >> SHIFT) ^ *current++)]; + } + } + else + { + static crcpp_constexpr CRCType SHIFT(BoundedConstexprValue(CHAR_BIT - CRCWidth)); + + remainder <<= SHIFT; + + while (size--) + { + // Note: no need to mask here since remainder is guaranteed to fit in a single byte. + remainder = lookupTable[static_cast(remainder ^ *current++)]; + } + + remainder >>= SHIFT; + } + + return remainder; + } + + /** + @brief Function to force a compile-time expression to be >= 0. + @note This function is used to avoid compiler warnings because all constexpr values are evaluated + in a function even in a branch will never be executed. This also means we don't need pragmas + to get rid of warnings, but it still can be computed at compile-time. Win-win! + @param[in] x Compile-time expression to bound + @tparam CRCType Integer type for storing the CRC result + @tparam CRCWidth Number of bits in the CRC + @return Non-negative compile-time expression + */ + template + inline crcpp_constexpr IntegerType CRC::BoundedConstexprValue(IntegerType x) + { + return (x < IntegerType(0)) ? IntegerType(0) : x; + } + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + /** + @brief Returns a set of parameters for CRC-4 ITU. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-4 ITU has the following parameters and check value: + - polynomial = 0x3 + - initial value = 0x0 + - final XOR = 0x0 + - reflect input = true + - reflect output = true + - check value = 0x7 + @return CRC-4 ITU parameters + */ + inline const CRC::Parameters & CRC::CRC_4_ITU() + { + static const Parameters parameters = { 0x3, 0x0, 0x0, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-5 EPC. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-5 EPC has the following parameters and check value: + - polynomial = 0x09 + - initial value = 0x09 + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0x00 + @return CRC-5 EPC parameters + */ + inline const CRC::Parameters & CRC::CRC_5_EPC() + { + static const Parameters parameters = { 0x09, 0x09, 0x00, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-5 ITU. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-5 ITU has the following parameters and check value: + - polynomial = 0x15 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0x07 + @return CRC-5 ITU parameters + */ + inline const CRC::Parameters & CRC::CRC_5_ITU() + { + static const Parameters parameters = { 0x15, 0x00, 0x00, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-5 USB. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-5 USB has the following parameters and check value: + - polynomial = 0x05 + - initial value = 0x1F + - final XOR = 0x1F + - reflect input = true + - reflect output = true + - check value = 0x19 + @return CRC-5 USB parameters + */ + inline const CRC::Parameters & CRC::CRC_5_USB() + { + static const Parameters parameters = { 0x05, 0x1F, 0x1F, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-6 CDMA2000-A. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-6 CDMA2000-A has the following parameters and check value: + - polynomial = 0x27 + - initial value = 0x3F + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0x0D + @return CRC-6 CDMA2000-A parameters + */ + inline const CRC::Parameters & CRC::CRC_6_CDMA2000A() + { + static const Parameters parameters = { 0x27, 0x3F, 0x00, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-6 CDMA2000-B. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-6 CDMA2000-A has the following parameters and check value: + - polynomial = 0x07 + - initial value = 0x3F + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0x3B + @return CRC-6 CDMA2000-B parameters + */ + inline const CRC::Parameters & CRC::CRC_6_CDMA2000B() + { + static const Parameters parameters = { 0x07, 0x3F, 0x00, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-6 ITU. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-6 ITU has the following parameters and check value: + - polynomial = 0x03 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0x06 + @return CRC-6 ITU parameters + */ + inline const CRC::Parameters & CRC::CRC_6_ITU() + { + static const Parameters parameters = { 0x03, 0x00, 0x00, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-7 JEDEC. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-7 JEDEC has the following parameters and check value: + - polynomial = 0x09 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0x75 + @return CRC-7 JEDEC parameters + */ + inline const CRC::Parameters & CRC::CRC_7() + { + static const Parameters parameters = { 0x09, 0x00, 0x00, false, false }; + return parameters; + } +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + + /** + @brief Returns a set of parameters for CRC-8 SMBus. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-8 SMBus has the following parameters and check value: + - polynomial = 0x07 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = false + - reflect output = false + - check value = 0xF4 + @return CRC-8 SMBus parameters + */ + inline const CRC::Parameters & CRC::CRC_8() + { + static const Parameters parameters = { 0x07, 0x00, 0x00, false, false }; + return parameters; + } + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + /** + @brief Returns a set of parameters for CRC-8 EBU (aka CRC-8 AES). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-8 EBU has the following parameters and check value: + - polynomial = 0x1D + - initial value = 0xFF + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0x97 + @return CRC-8 EBU parameters + */ + inline const CRC::Parameters & CRC::CRC_8_EBU() + { + static const Parameters parameters = { 0x1D, 0xFF, 0x00, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-8 MAXIM (aka CRC-8 DOW-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-8 MAXIM has the following parameters and check value: + - polynomial = 0x31 + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0xA1 + @return CRC-8 MAXIM parameters + */ + inline const CRC::Parameters & CRC::CRC_8_MAXIM() + { + static const Parameters parameters = { 0x31, 0x00, 0x00, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-8 WCDMA. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-8 WCDMA has the following parameters and check value: + - polynomial = 0x9B + - initial value = 0x00 + - final XOR = 0x00 + - reflect input = true + - reflect output = true + - check value = 0x25 + @return CRC-8 WCDMA parameters + */ + inline const CRC::Parameters & CRC::CRC_8_WCDMA() + { + static const Parameters parameters = { 0x9B, 0x00, 0x00, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-10 ITU. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-10 ITU has the following parameters and check value: + - polynomial = 0x233 + - initial value = 0x000 + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0x199 + @return CRC-10 ITU parameters + */ + inline const CRC::Parameters & CRC::CRC_10() + { + static const Parameters parameters = { 0x233, 0x000, 0x000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-10 CDMA2000. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-10 CDMA2000 has the following parameters and check value: + - polynomial = 0x3D9 + - initial value = 0x3FF + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0x233 + @return CRC-10 CDMA2000 parameters + */ + inline const CRC::Parameters & CRC::CRC_10_CDMA2000() + { + static const Parameters parameters = { 0x3D9, 0x3FF, 0x000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-11 FlexRay. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-11 FlexRay has the following parameters and check value: + - polynomial = 0x385 + - initial value = 0x01A + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0x5A3 + @return CRC-11 FlexRay parameters + */ + inline const CRC::Parameters & CRC::CRC_11() + { + static const Parameters parameters = { 0x385, 0x01A, 0x000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-12 CDMA2000. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-12 CDMA2000 has the following parameters and check value: + - polynomial = 0xF13 + - initial value = 0xFFF + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0xD4D + @return CRC-12 CDMA2000 parameters + */ + inline const CRC::Parameters & CRC::CRC_12_CDMA2000() + { + static const Parameters parameters = { 0xF13, 0xFFF, 0x000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-12 DECT (aka CRC-12 X-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-12 DECT has the following parameters and check value: + - polynomial = 0x80F + - initial value = 0x000 + - final XOR = 0x000 + - reflect input = false + - reflect output = false + - check value = 0xF5B + @return CRC-12 DECT parameters + */ + inline const CRC::Parameters & CRC::CRC_12_DECT() + { + static const Parameters parameters = { 0x80F, 0x000, 0x000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-12 UMTS (aka CRC-12 3GPP). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-12 UMTS has the following parameters and check value: + - polynomial = 0x80F + - initial value = 0x000 + - final XOR = 0x000 + - reflect input = false + - reflect output = true + - check value = 0xDAF + @return CRC-12 UMTS parameters + */ + inline const CRC::Parameters & CRC::CRC_12_UMTS() + { + static const Parameters parameters = { 0x80F, 0x000, 0x000, false, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-13 BBC. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-13 BBC has the following parameters and check value: + - polynomial = 0x1CF5 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x04FA + @return CRC-13 BBC parameters + */ + inline const CRC::Parameters & CRC::CRC_13_BBC() + { + static const Parameters parameters = { 0x1CF5, 0x0000, 0x0000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-15 CAN. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-15 CAN has the following parameters and check value: + - polynomial = 0x4599 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x059E + @return CRC-15 CAN parameters + */ + inline const CRC::Parameters & CRC::CRC_15() + { + static const Parameters parameters = { 0x4599, 0x0000, 0x0000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-15 MPT1327. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-15 MPT1327 has the following parameters and check value: + - polynomial = 0x6815 + - initial value = 0x0000 + - final XOR = 0x0001 + - reflect input = false + - reflect output = false + - check value = 0x2566 + @return CRC-15 MPT1327 parameters + */ + inline const CRC::Parameters & CRC::CRC_15_MPT1327() + { + static const Parameters parameters = { 0x6815, 0x0000, 0x0001, false, false }; + return parameters; + } +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + + /** + @brief Returns a set of parameters for CRC-16 ARC (aka CRC-16 IBM, CRC-16 LHA). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 ARC has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = true + - reflect output = true + - check value = 0xBB3D + @return CRC-16 ARC parameters + */ + inline const CRC::Parameters & CRC::CRC_16_ARC() + { + static const Parameters parameters = { 0x8005, 0x0000, 0x0000, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 BUYPASS (aka CRC-16 VERIFONE, CRC-16 UMTS). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 BUYPASS has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0xFEE8 + @return CRC-16 BUYPASS parameters + */ + inline const CRC::Parameters & CRC::CRC_16_BUYPASS() + { + static const Parameters parameters = { 0x8005, 0x0000, 0x0000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 CCITT FALSE. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 CCITT FALSE has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0xFFFF + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x29B1 + @return CRC-16 CCITT FALSE parameters + */ + inline const CRC::Parameters & CRC::CRC_16_CCITTFALSE() + { + static const Parameters parameters = { 0x1021, 0xFFFF, 0x0000, false, false }; + return parameters; + } + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + /** + @brief Returns a set of parameters for CRC-16 CDMA2000. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 CDMA2000 has the following parameters and check value: + - polynomial = 0xC867 + - initial value = 0xFFFF + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x4C06 + @return CRC-16 CDMA2000 parameters + */ + inline const CRC::Parameters & CRC::CRC_16_CDMA2000() + { + static const Parameters parameters = { 0xC867, 0xFFFF, 0x0000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 DECT-R (aka CRC-16 R-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 DECT-R has the following parameters and check value: + - polynomial = 0x0589 + - initial value = 0x0000 + - final XOR = 0x0001 + - reflect input = false + - reflect output = false + - check value = 0x007E + @return CRC-16 DECT-R parameters + */ + inline const CRC::Parameters & CRC::CRC_16_DECTR() + { + static const Parameters parameters = { 0x0589, 0x0000, 0x0001, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 DECT-X (aka CRC-16 X-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 DECT-X has the following parameters and check value: + - polynomial = 0x0589 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x007F + @return CRC-16 DECT-X parameters + */ + inline const CRC::Parameters & CRC::CRC_16_DECTX() + { + static const Parameters parameters = { 0x0589, 0x0000, 0x0000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 DNP. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 DNP has the following parameters and check value: + - polynomial = 0x3D65 + - initial value = 0x0000 + - final XOR = 0xFFFF + - reflect input = true + - reflect output = true + - check value = 0xEA82 + @return CRC-16 DNP parameters + */ + inline const CRC::Parameters & CRC::CRC_16_DNP() + { + static const Parameters parameters = { 0x3D65, 0x0000, 0xFFFF, true, true }; + return parameters; + } +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + + /** + @brief Returns a set of parameters for CRC-16 GENIBUS (aka CRC-16 EPC, CRC-16 I-CODE, CRC-16 DARC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 GENIBUS has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0xFFFF + - final XOR = 0xFFFF + - reflect input = false + - reflect output = false + - check value = 0xD64E + @return CRC-16 GENIBUS parameters + */ + inline const CRC::Parameters & CRC::CRC_16_GENIBUS() + { + static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 KERMIT (aka CRC-16 CCITT, CRC-16 CCITT-TRUE). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 KERMIT has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = true + - reflect output = true + - check value = 0x2189 + @return CRC-16 KERMIT parameters + */ + inline const CRC::Parameters & CRC::CRC_16_KERMIT() + { + static const Parameters parameters = { 0x1021, 0x0000, 0x0000, true, true }; + return parameters; + } + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + /** + @brief Returns a set of parameters for CRC-16 MAXIM. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 MAXIM has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0x0000 + - final XOR = 0xFFFF + - reflect input = true + - reflect output = true + - check value = 0x44C2 + @return CRC-16 MAXIM parameters + */ + inline const CRC::Parameters & CRC::CRC_16_MAXIM() + { + static const Parameters parameters = { 0x8005, 0x0000, 0xFFFF, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 MODBUS. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 MODBUS has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0xFFFF + - final XOR = 0x0000 + - reflect input = true + - reflect output = true + - check value = 0x4B37 + @return CRC-16 MODBUS parameters + */ + inline const CRC::Parameters & CRC::CRC_16_MODBUS() + { + static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 T10-DIF. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 T10-DIF has the following parameters and check value: + - polynomial = 0x8BB7 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0xD0DB + @return CRC-16 T10-DIF parameters + */ + inline const CRC::Parameters & CRC::CRC_16_T10DIF() + { + static const Parameters parameters = { 0x8BB7, 0x0000, 0x0000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 USB. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 USB has the following parameters and check value: + - polynomial = 0x8005 + - initial value = 0xFFFF + - final XOR = 0xFFFF + - reflect input = true + - reflect output = true + - check value = 0xB4C8 + @return CRC-16 USB parameters + */ + inline const CRC::Parameters & CRC::CRC_16_USB() + { + static const Parameters parameters = { 0x8005, 0xFFFF, 0xFFFF, true, true }; + return parameters; + } +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + + /** + @brief Returns a set of parameters for CRC-16 X-25 (aka CRC-16 IBM-SDLC, CRC-16 ISO-HDLC, CRC-16 B). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 X-25 has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0xFFFF + - final XOR = 0xFFFF + - reflect input = true + - reflect output = true + - check value = 0x906E + @return CRC-16 X-25 parameters + */ + inline const CRC::Parameters & CRC::CRC_16_X25() + { + static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-16 XMODEM (aka CRC-16 ZMODEM, CRC-16 ACORN, CRC-16 LTE). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-16 XMODEM has the following parameters and check value: + - polynomial = 0x1021 + - initial value = 0x0000 + - final XOR = 0x0000 + - reflect input = false + - reflect output = false + - check value = 0x31C3 + @return CRC-16 XMODEM parameters + */ + inline const CRC::Parameters & CRC::CRC_16_XMODEM() + { + static const Parameters parameters = { 0x1021, 0x0000, 0x0000, false, false }; + return parameters; + } + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + /** + @brief Returns a set of parameters for CRC-17 CAN. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-17 CAN has the following parameters and check value: + - polynomial = 0x1685B + - initial value = 0x00000 + - final XOR = 0x00000 + - reflect input = false + - reflect output = false + - check value = 0x04F03 + @return CRC-17 CAN parameters + */ + inline const CRC::Parameters & CRC::CRC_17_CAN() + { + static const Parameters parameters = { 0x1685B, 0x00000, 0x00000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-21 CAN. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-21 CAN has the following parameters and check value: + - polynomial = 0x102899 + - initial value = 0x000000 + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0x0ED841 + @return CRC-21 CAN parameters + */ + inline const CRC::Parameters & CRC::CRC_21_CAN() + { + static const Parameters parameters = { 0x102899, 0x000000, 0x000000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-24 OPENPGP. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-24 OPENPGP has the following parameters and check value: + - polynomial = 0x864CFB + - initial value = 0xB704CE + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0x21CF02 + @return CRC-24 OPENPGP parameters + */ + inline const CRC::Parameters & CRC::CRC_24() + { + static const Parameters parameters = { 0x864CFB, 0xB704CE, 0x000000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-24 FlexRay-A. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-24 FlexRay-A has the following parameters and check value: + - polynomial = 0x5D6DCB + - initial value = 0xFEDCBA + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0x7979BD + @return CRC-24 FlexRay-A parameters + */ + inline const CRC::Parameters & CRC::CRC_24_FLEXRAYA() + { + static const Parameters parameters = { 0x5D6DCB, 0xFEDCBA, 0x000000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-24 FlexRay-B. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-24 FlexRay-B has the following parameters and check value: + - polynomial = 0x5D6DCB + - initial value = 0xABCDEF + - final XOR = 0x000000 + - reflect input = false + - reflect output = false + - check value = 0x1F23B8 + @return CRC-24 FlexRay-B parameters + */ + inline const CRC::Parameters & CRC::CRC_24_FLEXRAYB() + { + static const Parameters parameters = { 0x5D6DCB, 0xABCDEF, 0x000000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-30 CDMA. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-30 CDMA has the following parameters and check value: + - polynomial = 0x2030B9C7 + - initial value = 0x3FFFFFFF + - final XOR = 0x00000000 + - reflect input = false + - reflect output = false + - check value = 0x3B3CB540 + @return CRC-30 CDMA parameters + */ + inline const CRC::Parameters & CRC::CRC_30() + { + static const Parameters parameters = { 0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false }; + return parameters; + } +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + + /** + @brief Returns a set of parameters for CRC-32 (aka CRC-32 ADCCP, CRC-32 PKZip). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 has the following parameters and check value: + - polynomial = 0x04C11DB7 + - initial value = 0xFFFFFFFF + - final XOR = 0xFFFFFFFF + - reflect input = true + - reflect output = true + - check value = 0xCBF43926 + @return CRC-32 parameters + */ + inline const CRC::Parameters & CRC::CRC_32() + { + static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-32 BZIP2 (aka CRC-32 AAL5, CRC-32 DECT-B, CRC-32 B-CRC). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 BZIP2 has the following parameters and check value: + - polynomial = 0x04C11DB7 + - initial value = 0xFFFFFFFF + - final XOR = 0xFFFFFFFF + - reflect input = false + - reflect output = false + - check value = 0xFC891918 + @return CRC-32 BZIP2 parameters + */ + inline const CRC::Parameters & CRC::CRC_32_BZIP2() + { + static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, false, false }; + return parameters; + } + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + /** + @brief Returns a set of parameters for CRC-32 C (aka CRC-32 ISCSI, CRC-32 Castagnoli, CRC-32 Interlaken). + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 C has the following parameters and check value: + - polynomial = 0x1EDC6F41 + - initial value = 0xFFFFFFFF + - final XOR = 0xFFFFFFFF + - reflect input = true + - reflect output = true + - check value = 0xE3069283 + @return CRC-32 C parameters + */ + inline const CRC::Parameters & CRC::CRC_32_C() + { + static const Parameters parameters = { 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; + return parameters; + } +#endif + + /** + @brief Returns a set of parameters for CRC-32 MPEG-2. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 MPEG-2 has the following parameters and check value: + - polynomial = 0x04C11DB7 + - initial value = 0xFFFFFFFF + - final XOR = 0x00000000 + - reflect input = false + - reflect output = false + - check value = 0x0376E6E7 + @return CRC-32 MPEG-2 parameters + */ + inline const CRC::Parameters & CRC::CRC_32_MPEG2() + { + static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0x00000000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-32 POSIX. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 POSIX has the following parameters and check value: + - polynomial = 0x04C11DB7 + - initial value = 0x00000000 + - final XOR = 0xFFFFFFFF + - reflect input = false + - reflect output = false + - check value = 0x765E7680 + @return CRC-32 POSIX parameters + */ + inline const CRC::Parameters & CRC::CRC_32_POSIX() + { + static const Parameters parameters = { 0x04C11DB7, 0x00000000, 0xFFFFFFFF, false, false }; + return parameters; + } + +#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + /** + @brief Returns a set of parameters for CRC-32 Q. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-32 Q has the following parameters and check value: + - polynomial = 0x814141AB + - initial value = 0x00000000 + - final XOR = 0x00000000 + - reflect input = false + - reflect output = false + - check value = 0x3010BF7F + @return CRC-32 Q parameters + */ + inline const CRC::Parameters & CRC::CRC_32_Q() + { + static const Parameters parameters = { 0x814141AB, 0x00000000, 0x00000000, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-40 GSM. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-40 GSM has the following parameters and check value: + - polynomial = 0x0004820009 + - initial value = 0x0000000000 + - final XOR = 0xFFFFFFFFFF + - reflect input = false + - reflect output = false + - check value = 0xD4164FC646 + @return CRC-40 GSM parameters + */ + inline const CRC::Parameters & CRC::CRC_40_GSM() + { + static const Parameters parameters = { 0x0004820009, 0x0000000000, 0xFFFFFFFFFF, false, false }; + return parameters; + } + + /** + @brief Returns a set of parameters for CRC-64 ECMA. + @note The parameters are static and are delayed-constructed to reduce memory footprint. + @note CRC-64 ECMA has the following parameters and check value: + - polynomial = 0x42F0E1EBA9EA3693 + - initial value = 0x0000000000000000 + - final XOR = 0x0000000000000000 + - reflect input = false + - reflect output = false + - check value = 0x6C40DF5F0B497347 + @return CRC-64 ECMA parameters + */ + inline const CRC::Parameters & CRC::CRC_64() + { + static const Parameters parameters = { 0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false }; + return parameters; + } +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS + +#ifdef CRCPP_USE_NAMESPACE +} +#endif + +#endif // CRCPP_CRC_H_ \ No newline at end of file diff --git a/rpcs3/DS4PadHandler.cpp b/rpcs3/DS4PadHandler.cpp index 8b788caaa4..6ff4e1a9e3 100644 --- a/rpcs3/DS4PadHandler.cpp +++ b/rpcs3/DS4PadHandler.cpp @@ -8,9 +8,18 @@ namespace { - const u32 THREAD_TIMEOUT = 1000; - const u32 THREAD_SLEEP = 1; //ds4 has new data every ~4ms, - const u32 THREAD_SLEEP_INACTIVE = 100; + const auto THREAD_SLEEP = 1ms; //ds4 has new data every ~4ms, + const auto THREAD_SLEEP_INACTIVE = 100ms; + + const u32 DS4_ACC_RES_PER_G = 8192; + const u32 DS4_GYRO_RES_PER_DEG_S = 16; // technically this could be 1024, but keeping it at 16 keeps us within 16 bits of precision + const u32 DS4_FEATURE_REPORT_0x02_SIZE = 37; + const u32 DS4_FEATURE_REPORT_0x05_SIZE = 41; + const u32 DS4_FEATURE_REPORT_0x81_SIZE = 7; + const u32 DS4_INPUT_REPORT_0x11_SIZE = 78; + const u32 DS4_OUTPUT_REPORT_0x05_SIZE = 32; + const u32 DS4_OUTPUT_REPORT_0x11_SIZE = 78; + const u32 DS4_INPUT_REPORT_GYRO_X_OFFSET = 13; inline u16 Clamp0To255(f32 input) { @@ -102,6 +111,16 @@ namespace return std::tuple(Clamp0To255((outX + 1) * 127.f), Clamp0To255(((outY * -1) + 1) * 127.f)); }*/ + + inline s16 GetS16LEData(const u8* buf) + { + return (s16)(((u16)buf[0] << 0) + ((u16)buf[1] << 8)); + } + + inline u32 GetU32LEData(const u8* buf) + { + return (u32)(((u32)buf[0] << 0) + ((u32)buf[1] << 8) + ((u32)buf[2] << 16) + ((u32)buf[3] << 24)); + } } DS4PadHandler::~DS4PadHandler() @@ -376,12 +395,13 @@ void DS4PadHandler::ProcessData() pad.m_buttons[12 + i - 4].m_value = pressed ? 255 : 0; } + // these values come already calibrated from our DS4Thread, + // all we need to do is convert to ds3 range + // accel - // todo: scaling and double check these - // *i think* this is the constant for getting accel into absolute 'g' format...also need to flip them - f32 accelX = (((s16)((u16)(buf[20] << 8) | buf[21])) / 8315.f) * -1; - f32 accelY = (((s16)((u16)(buf[22] << 8) | buf[23])) / 8315.f) * -1; - f32 accelZ = (((s16)((u16)(buf[24] << 8) | buf[25])) / 8315.f) * -1; + f32 accelX = (((s16)((u16)(buf[20] << 8) | buf[19])) / static_cast(DS4_ACC_RES_PER_G)) * -1; + f32 accelY = (((s16)((u16)(buf[22] << 8) | buf[21])) / static_cast(DS4_ACC_RES_PER_G)) * -1; + f32 accelZ = (((s16)((u16)(buf[24] << 8) | buf[23])) / static_cast(DS4_ACC_RES_PER_G)) * -1; // now just use formula from ds3 accelX = accelX * 113 + 512; @@ -392,12 +412,15 @@ void DS4PadHandler::ProcessData() pad.m_sensors[1].m_value = Clamp0To1023(accelY); pad.m_sensors[2].m_value = Clamp0To1023(accelZ); - // todo: scaling check - // gyroX looks to be yaw, which is what we need - const int gyroX = (((s16)((u16)(buf[16] << 8) | buf[17])) / 128) * -1; - //const int gyroY = ((u16)(buf[14] << 8) | buf[15]) / 256; - //const int gyroZ = ((u16)(buf[18] << 8) | buf[19]) / 256; - pad.m_sensors[3].m_value = Clamp0To1023(gyroX + 512); + // gyroX is yaw, which is all that we need + f32 gyroX = (((s16)((u16)(buf[16] << 8) | buf[15])) / static_cast(DS4_GYRO_RES_PER_DEG_S)) * -1; + //const int gyroY = ((u16)(buf[14] << 8) | buf[13]) / 256; + //const int gyroZ = ((u16)(buf[18] << 8) | buf[17]) / 256; + + // convert to ds3 + gyroX = gyroX * (123.f / 90.f) + 512; + + pad.m_sensors[3].m_value = Clamp0To1023(gyroX); i++; } @@ -428,8 +451,10 @@ void DS4Thread::SetRumbleData(u32 port, u8 largeVibrate, u8 smallVibrate) { if (i == port) { + controller.second.newVibrateData = controller.second.largeVibrate != largeVibrate || controller.second.smallVibrate != smallVibrate; controller.second.largeVibrate = largeVibrate; controller.second.smallVibrate = smallVibrate; + break; } ++i; } @@ -461,6 +486,127 @@ std::array, MAX_GAMEPADS> DS4Thread::GetControllerData() return rtnData; } +bool DS4Thread::GetCalibrationData(DS4Device* ds4Dev) +{ + std::array buf; + if (ds4Dev->btCon) + { + for (int tries = 0; tries < 3; ++tries) { + buf[0] = 0x05; + if (hid_get_feature_report(ds4Dev->hidDevice, buf.data(), DS4_FEATURE_REPORT_0x05_SIZE) <= 0) + return false; + + const u8 btHdr = 0xA3; + const u32 crcHdr = CRCPP::CRC::Calculate(&btHdr, 1, crcTable); + const u32 crcCalc = CRCPP::CRC::Calculate(buf.data(), (DS4_FEATURE_REPORT_0x05_SIZE - 4), crcTable, crcHdr); + const u32 crcReported = GetU32LEData(&buf[DS4_FEATURE_REPORT_0x05_SIZE - 4]); + if (crcCalc != crcReported) + LOG_WARNING(HLE, "[DS4] Calibration CRC check failed! Will retry up to 3 times. Received 0x%x, Expected 0x%x", crcReported, crcCalc); + else break; + if (tries == 2) + return false; + } + } + else + { + buf[0] = 0x02; + if (hid_get_feature_report(ds4Dev->hidDevice, buf.data(), DS4_FEATURE_REPORT_0x02_SIZE) <= 0) + return false; + } + + ds4Dev->calibData[DS4CalibIndex::PITCH].bias = GetS16LEData(&buf[1]); + ds4Dev->calibData[DS4CalibIndex::YAW].bias = GetS16LEData(&buf[3]); + ds4Dev->calibData[DS4CalibIndex::ROLL].bias = GetS16LEData(&buf[5]); + + s16 pitchPlus, pitchNeg, rollPlus, rollNeg, yawPlus, yawNeg; + if (ds4Dev->btCon) + { + pitchPlus = GetS16LEData(&buf[7]); + yawPlus = GetS16LEData(&buf[9]); + rollPlus = GetS16LEData(&buf[11]); + pitchNeg = GetS16LEData(&buf[13]); + yawNeg = GetS16LEData(&buf[15]); + rollNeg = GetS16LEData(&buf[17]); + } + else + { + pitchPlus = GetS16LEData(&buf[7]); + pitchNeg = GetS16LEData(&buf[9]); + yawPlus = GetS16LEData(&buf[11]); + yawNeg = GetS16LEData(&buf[13]); + rollPlus = GetS16LEData(&buf[15]); + rollNeg = GetS16LEData(&buf[17]); + } + + const s32 gyroSpeedScale = GetS16LEData(&buf[19]) + GetS16LEData(&buf[21]); + + ds4Dev->calibData[DS4CalibIndex::PITCH].sensNumer = gyroSpeedScale * DS4_GYRO_RES_PER_DEG_S; + ds4Dev->calibData[DS4CalibIndex::PITCH].sensDenom = pitchPlus - pitchNeg; + + ds4Dev->calibData[DS4CalibIndex::YAW].sensNumer = gyroSpeedScale * DS4_GYRO_RES_PER_DEG_S; + ds4Dev->calibData[DS4CalibIndex::YAW].sensDenom = yawPlus - yawNeg; + + ds4Dev->calibData[DS4CalibIndex::ROLL].sensNumer = gyroSpeedScale * DS4_GYRO_RES_PER_DEG_S; + ds4Dev->calibData[DS4CalibIndex::ROLL].sensDenom = rollPlus - rollNeg; + + const s16 accelXPlus = GetS16LEData(&buf[23]); + const s16 accelXNeg = GetS16LEData(&buf[25]); + const s16 accelYPlus = GetS16LEData(&buf[27]); + const s16 accelYNeg = GetS16LEData(&buf[29]); + const s16 accelZPlus = GetS16LEData(&buf[31]); + const s16 accelZNeg = GetS16LEData(&buf[33]); + + const s32 accelXRange = accelXPlus - accelXNeg; + ds4Dev->calibData[DS4CalibIndex::X].bias = accelXPlus - accelXRange / 2; + ds4Dev->calibData[DS4CalibIndex::X].sensNumer = 2 * DS4_ACC_RES_PER_G; + ds4Dev->calibData[DS4CalibIndex::X].sensDenom = accelXRange; + + const s32 accelYRange = accelYPlus - accelYNeg; + ds4Dev->calibData[DS4CalibIndex::Y].bias = accelYPlus - accelYRange / 2; + ds4Dev->calibData[DS4CalibIndex::Y].sensNumer = 2 * DS4_ACC_RES_PER_G; + ds4Dev->calibData[DS4CalibIndex::Y].sensDenom = accelYRange; + + const s32 accelZRange = accelZPlus - accelZNeg; + ds4Dev->calibData[DS4CalibIndex::Z].bias = accelZPlus - accelZRange / 2; + ds4Dev->calibData[DS4CalibIndex::Z].sensNumer = 2 * DS4_ACC_RES_PER_G; + ds4Dev->calibData[DS4CalibIndex::Z].sensDenom = accelZRange; + + return true; +} + +void DS4Thread::CheckAddDevice(hid_device* hidDevice, hid_device_info* hidDevInfo) +{ + std::string serial = ""; + DS4Device ds4Dev; + ds4Dev.hidDevice = hidDevice; + // There isnt a nice 'portable' way with hidapi to detect bt vs wired as the pid/vid's are the same + // Let's try getting 0x81 feature report, which should will return mac address on wired, and should error on bluetooth + std::array buf{}; + buf[0] = 0x81; + if (hid_get_feature_report(hidDevice, buf.data(), DS4_FEATURE_REPORT_0x81_SIZE) > 0) + { + serial = fmt::format("%x%x%x%x%x%x", buf[6], buf[5], buf[4], buf[3], buf[2], buf[1]); + } + else + { + ds4Dev.btCon = true; + std::wstring wSerial(hidDevInfo->serial_number); + serial = std::string(wSerial.begin(), wSerial.end()); + } + + if (!GetCalibrationData(&ds4Dev)) + { + LOG_ERROR(HLE, "[DS4] Failed getting calibration data, ignoring controller!"); + hid_close(hidDevice); + return; + } + + ds4Dev.path = hidDevInfo->path; + + hid_set_nonblocking(hidDevice, 1); + controllers.emplace(serial, ds4Dev); +} + void DS4Thread::on_init(const std::shared_ptr& _this) { const int res = hid_init(); @@ -479,29 +625,8 @@ void DS4Thread::on_init(const std::shared_ptr& _this) hid_device* dev = hid_open_path(devInfo->path); if (dev) - { - hid_set_nonblocking(dev, 1); - // There isnt a nice 'portable' way with hidapi to detect bt vs wired as the pid/vid's are the same - // Let's try getting 0x81 feature report, which should will return mac address on wired, and should error on bluetooth - std::array buf{}; - buf[0] = 0x81; - if (hid_get_feature_report(dev, buf.data(), buf.size()) > 0) - { - std::string serial = fmt::format("%x%x%x%x%x%x", buf[6], buf[5], buf[4], buf[3], buf[2], buf[1]); - controllers.emplace(serial, DS4Device{ dev, devInfo->path, false }); - } - else - { - // this kicks bt into sending the correct data - std::array buf{}; - buf[0] = 0x2; - hid_get_feature_report(dev, buf.data(), buf.size()); + CheckAddDevice(dev, devInfo); - std::wstring wSerial(devInfo->serial_number); - std::string serialNum = std::string(wSerial.begin(), wSerial.end()); - controllers.emplace(serialNum, DS4Device{ dev, devInfo->path, true}); - } - } devInfo = devInfo->next; } } @@ -524,6 +649,46 @@ DS4Thread::~DS4Thread() hid_exit(); } +void DS4Thread::SendVibrateData(const DS4Device& device) +{ + std::array outputBuf{0}; + // write rumble state + if (device.btCon) + { + outputBuf[0] = 0x11; + outputBuf[1] = 0xC4; + outputBuf[3] = 0x07; + outputBuf[6] = device.smallVibrate; + outputBuf[7] = device.largeVibrate; + outputBuf[8] = 0x00; // red + outputBuf[9] = 0x00; // green + outputBuf[10] = 0xff; // blue + + const u8 btHdr = 0xA2; + const u32 crcHdr = CRCPP::CRC::Calculate(&btHdr, 1, crcTable); + const u32 crcCalc = CRCPP::CRC::Calculate(outputBuf.data(), (DS4_OUTPUT_REPORT_0x11_SIZE - 4), crcTable, crcHdr); + + outputBuf[74] = (crcCalc >> 0) & 0xFF; + outputBuf[75] = (crcCalc >> 8) & 0xFF; + outputBuf[76] = (crcCalc >> 16) & 0xFF; + outputBuf[77] = (crcCalc >> 24) & 0xFF; + + hid_write_control(device.hidDevice, outputBuf.data(), DS4_OUTPUT_REPORT_0x11_SIZE); + } + else + { + outputBuf[0] = 0x05; + outputBuf[1] = 0x07; + outputBuf[4] = device.smallVibrate; + outputBuf[5] = device.largeVibrate; + outputBuf[6] = 0x00; // red + outputBuf[7] = 0x00; // green + outputBuf[8] = 0xff; // blue + + hid_write(device.hidDevice, outputBuf.data(), DS4_OUTPUT_REPORT_0x05_SIZE); + } +} + void DS4Thread::on_task() { while (!Emu.IsStopped()) @@ -537,9 +702,8 @@ void DS4Thread::on_task() u32 online = 0; u32 i = 0; - std::array buf{}; - std::array btBuf{}; - std::array outputBuf{0}; + std::array buf{}; + for (auto & controller : controllers) { @@ -547,14 +711,14 @@ void DS4Thread::on_task() if (controller.second.hidDevice == nullptr) { - // try to connect + // try to reconnect hid_device* dev = hid_open_path(controller.second.path.c_str()); if (dev) { hid_set_nonblocking(dev, 1); if (controller.second.btCon) { - // this kicks bt into sending the correct data + // We already have calibration data, but we still need this to kick BT into sending correct 0x11 reports std::array buf{}; buf[0] = 0x2; hid_get_feature_report(dev, buf.data(), buf.size()); @@ -570,81 +734,59 @@ void DS4Thread::on_task() online++; - if (controller.second.btCon) + const int res = hid_read(controller.second.hidDevice, buf.data(), controller.second.btCon ? 78 : 64); + if (res == -1) { - const int res = hid_read(controller.second.hidDevice, btBuf.data(), btBuf.size()); - if (res == -1) - { - // looks like controller disconnected or read error, deal with it on next loop - hid_close(controller.second.hidDevice); - controller.second.hidDevice = nullptr; + // looks like controller disconnected or read error, deal with it on next loop + hid_close(controller.second.hidDevice); + controller.second.hidDevice = nullptr; + continue; + } + + // no data? keep going + if (res == 0) + continue; + + int offset = 0; + // check report and set offset + if (controller.second.btCon && buf[0] == 0x11 && res == 78) + { + offset = 2; + + const u8 btHdr = 0xA1; + const u32 crcHdr = CRCPP::CRC::Calculate(&btHdr, 1, crcTable); + const u32 crcCalc = CRCPP::CRC::Calculate(buf.data(), (DS4_INPUT_REPORT_0x11_SIZE - 4), crcTable, crcHdr); + const u32 crcReported = GetU32LEData(&buf[DS4_INPUT_REPORT_0x11_SIZE - 4]); + if (crcCalc != crcReported) { + LOG_WARNING(HLE, "[DS4] Data packet CRC check failed, ignoring! Received 0x%x, Expected 0x%x", crcReported, crcCalc); continue; } - // no data? keep going - if (res == 0) - continue; - - // not the report we want - if (btBuf[0] != 0x11) - continue; - - if (res != 67) - fmt::throw_exception("unexpected ds4 bt packet size"); - - // shave off first two bytes that are bluetooth specific - memcpy(padData[i].data(), &btBuf[2], 64); } + else if (!controller.second.btCon && buf[0] == 0x01 && res == 64) + offset = 0; else + continue; + + int calibOffset = offset + DS4_INPUT_REPORT_GYRO_X_OFFSET; + for (int i = 0; i < DS4CalibIndex::COUNT; ++i) { - const int res = hid_read(controller.second.hidDevice, buf.data(), buf.size()); - if (res == -1 || (res != 0 && res != 64)) - { - // looks like controller disconnected or read error, deal with it on next loop - hid_close(controller.second.hidDevice); - controller.second.hidDevice = nullptr; - continue; - } - - // no data? keep going - if (res == 0) - continue; - - memcpy(padData[i].data(), buf.data(), 64); + const s16 rawValue = GetS16LEData(&buf[calibOffset]); + const s16 calValue = ApplyCalibration(rawValue, controller.second.calibData[i]); + buf[calibOffset++] = ((u16)calValue >> 0) & 0xFF; + buf[calibOffset++] = ((u16)calValue >> 8) & 0xFF; } - outputBuf.fill(0); + memcpy(padData[i].data(), &buf[offset], 64); - // write rumble state - if (controller.second.btCon) + if (controller.second.newVibrateData) { - outputBuf[0] = 0x11; - outputBuf[1] = 0x80; - outputBuf[3] = 0xff; - outputBuf[6] = controller.second.smallVibrate; - outputBuf[7] = controller.second.largeVibrate; - outputBuf[8] = 0x00; // red - outputBuf[9] = 0x00; // green - outputBuf[10] = 0xff; // blue - - hid_write_control(controller.second.hidDevice, outputBuf.data(), 78); + SendVibrateData(controller.second); + controller.second.newVibrateData = false; } - else - { - outputBuf[0] = 0x05; - outputBuf[1] = 0xff; - outputBuf[4] = controller.second.smallVibrate; - outputBuf[5] = controller.second.largeVibrate; - outputBuf[6] = 0x00; // red - outputBuf[7] = 0x00; // green - outputBuf[8] = 0xff; // blue - - hid_write(controller.second.hidDevice, outputBuf.data(), 64); - } - i++; } - std::this_thread::sleep_for((online > 0) ? 1ms : 100ms); + std::this_thread::sleep_for((online > 0) ? THREAD_SLEEP : THREAD_SLEEP_INACTIVE); } } \ No newline at end of file diff --git a/rpcs3/DS4PadHandler.h b/rpcs3/DS4PadHandler.h index 1289fc09fa..74c712b48a 100644 --- a/rpcs3/DS4PadHandler.h +++ b/rpcs3/DS4PadHandler.h @@ -2,20 +2,45 @@ #include "Emu/Io/PadHandler.h" #include "Utilities/Thread.h" +#include "Utilities/CRC.h" #include "hidapi.h" +#include const u32 MAX_GAMEPADS = 7; class DS4Thread final : public named_thread { private: + enum DS4CalibIndex + { + // gyro + PITCH = 0, + YAW, + ROLL, + + // accel + X, + Y, + Z, + COUNT + }; + + struct DS4CalibData + { + s16 bias; + s32 sensNumer; + s32 sensDenom; + }; + struct DS4Device { - hid_device* hidDevice; - std::string path; - bool btCon; - u8 largeVibrate; - u8 smallVibrate; + hid_device* hidDevice{ nullptr }; + std::string path{ "" }; + bool btCon{ false }; + std::array calibData; + bool newVibrateData{true}; + u8 largeVibrate{0}; + u8 smallVibrate{0}; }; const u16 DS4_VID = 0x054C; @@ -34,6 +59,8 @@ private: semaphore<> mutex; + CRCPP::CRC::Table crcTable{ CRCPP::CRC::CRC_32() }; + public: void on_init(const std::shared_ptr&) override; @@ -46,6 +73,24 @@ public: DS4Thread() = default; ~DS4Thread(); + +private: + bool GetCalibrationData(DS4Device* ds4Device); + void CheckAddDevice(hid_device* hidDevice, hid_device_info* hidDevInfo); + void SendVibrateData(const DS4Device& device); + inline s16 ApplyCalibration(s32 rawValue, const DS4CalibData& calibData) + { + const s32 biased = rawValue - calibData.bias; + const s32 quot = calibData.sensNumer / calibData.sensDenom; + const s32 rem = calibData.sensNumer % calibData.sensDenom; + const s32 output = (quot * biased) + ((rem * biased) / calibData.sensDenom); + + if (output > std::numeric_limits::max()) + return std::numeric_limits::max(); + else if (output < std::numeric_limits::min()) + return std::numeric_limits::min(); + else return static_cast(output); + } }; class DS4PadHandler final : public PadHandlerBase diff --git a/rpcs3/emucore.vcxproj b/rpcs3/emucore.vcxproj index e5be30b14c..c97027e4a1 100644 --- a/rpcs3/emucore.vcxproj +++ b/rpcs3/emucore.vcxproj @@ -409,6 +409,7 @@ + diff --git a/rpcs3/emucore.vcxproj.filters b/rpcs3/emucore.vcxproj.filters index 0e54e5c8da..c6647546ee 100644 --- a/rpcs3/emucore.vcxproj.filters +++ b/rpcs3/emucore.vcxproj.filters @@ -1774,5 +1774,8 @@ Emu\Cell\lv2 + + Utilities + \ No newline at end of file