diff options
Diffstat (limited to 'src/core/hle/service/nwm/uds_data.cpp')
-rw-r--r-- | src/core/hle/service/nwm/uds_data.cpp | 80 |
1 files changed, 47 insertions, 33 deletions
diff --git a/src/core/hle/service/nwm/uds_data.cpp b/src/core/hle/service/nwm/uds_data.cpp index e05ca8815..fabdf67a8 100644 --- a/src/core/hle/service/nwm/uds_data.cpp +++ b/src/core/hle/service/nwm/uds_data.cpp @@ -3,20 +3,20 @@ // Refer to the license.txt file included. #include <cstring> - -#include "core/hle/service/nwm/nwm_uds.h" -#include "core/hle/service/nwm/uds_beacon.h" -#include "core/hle/service/nwm/uds_data.h" -#include "core/hw/aes/key.h" - +#include <cryptopp/aes.h> #include <cryptopp/ccm.h> #include <cryptopp/filters.h> #include <cryptopp/md5.h> #include <cryptopp/modes.h> +#include "core/hle/service/nwm/nwm_uds.h" +#include "core/hle/service/nwm/uds_data.h" +#include "core/hw/aes/key.h" namespace Service { namespace NWM { +using MacAddress = std::array<u8, 6>; + // AES Keyslot used to generate the UDS data frame CCMP key. constexpr size_t UDSDataCryptoAESKeySlot = 0x2D; @@ -39,14 +39,15 @@ static std::vector<u8> GenerateLLCHeader(EtherType protocol) { * @returns a buffer with the bytes of the generated header. */ static std::vector<u8> GenerateSecureDataHeader(u16 data_size, u8 channel, u16 dest_node_id, - u16 src_node_id, u16 sequence_number) { + u16 src_node_id, u16 sequence_number) { SecureDataHeader header{}; header.protocol_size = data_size + sizeof(SecureDataHeader); // Note: This size includes everything except the first 4 bytes of the structure, // reinforcing the hypotheses that the first 4 bytes are actually the header of // another container protocol. header.securedata_size = data_size + sizeof(SecureDataHeader) - 4; - header.is_management = 0; // Frames sent by the emulated application are never UDS management frames + // Frames sent by the emulated application are never UDS management frames + header.is_management = 0; header.data_channel = channel; header.sequence_number = sequence_number; header.dest_node_id = dest_node_id; @@ -60,7 +61,7 @@ static std::vector<u8> GenerateSecureDataHeader(u16 data_size, u8 channel, u16 d /* * Calculates the CTR used for the AES-CTR process that calculates - * the CCMP crypto key for data frames. + * the CCMP crypto key for data frames. * @returns The CTR used for data frames crypto key generation. */ static std::array<u8, CryptoPP::MD5::DIGESTSIZE> GetDataCryptoCTR(const NetworkInfo& network_info) { @@ -81,15 +82,16 @@ static std::array<u8, CryptoPP::MD5::DIGESTSIZE> GetDataCryptoCTR(const NetworkI * Generates the key used for encrypting the 802.11 data frames generated by UDS. * @returns The key used for data frames crypto. */ -static std::array<u8, CryptoPP::AES::BLOCKSIZE> GenerateDataCCMPKey(const std::vector<u8>& passphrase, - const NetworkInfo& network_info) { +static std::array<u8, CryptoPP::AES::BLOCKSIZE> GenerateDataCCMPKey( + const std::vector<u8>& passphrase, const NetworkInfo& network_info) { // Calculate the MD5 hash of the input passphrase. std::array<u8, CryptoPP::MD5::DIGESTSIZE> passphrase_hash; CryptoPP::MD5().CalculateDigest(passphrase_hash.data(), passphrase.data(), passphrase.size()); std::array<u8, CryptoPP::AES::BLOCKSIZE> ccmp_key; - // The CCMP key is the result of encrypting the MD5 hash of the passphrase with AES-CTR using keyslot 0x2D. + // The CCMP key is the result of encrypting the MD5 hash of the passphrase with AES-CTR using + // keyslot 0x2D. using CryptoPP::AES; std::array<u8, CryptoPP::MD5::DIGESTSIZE> counter = GetDataCryptoCTR(network_info); std::array<u8, AES::BLOCKSIZE> key = HW::AES::GetNormalKey(UDSDataCryptoAESKeySlot); @@ -139,21 +141,26 @@ static std::vector<u8> GenerateCCMPAAD(const MacAddress& sender, const MacAddres * Decrypts the payload of an encrypted 802.11 data frame using the specified key. * @returns The decrypted payload. */ -static std::vector<u8> DecryptDataFrame(const std::vector<u8>& encrypted_payload, const std::array<u8, CryptoPP::AES::BLOCKSIZE>& ccmp_key, - const MacAddress& sender, const MacAddress& receiver, u16 sequence_number) { +static std::vector<u8> DecryptDataFrame(const std::vector<u8>& encrypted_payload, + const std::array<u8, CryptoPP::AES::BLOCKSIZE>& ccmp_key, + const MacAddress& sender, const MacAddress& receiver, + u16 sequence_number) { // Reference: IEEE 802.11-2007 std::vector<u8> aad = GenerateCCMPAAD(sender, receiver); - std::vector<u8> packet_number{0, 0, 0, 0, + std::vector<u8> packet_number{0, + 0, + 0, + 0, static_cast<u8>((sequence_number >> 8) & 0xFF), static_cast<u8>(sequence_number & 0xFF)}; // 8.3.3.3.3 Construct CCM nonce (13 bytes) std::vector<u8> nonce; - nonce.push_back(0); // priority - nonce.insert(nonce.end(), sender.begin(), sender.end()); // Address 2 + nonce.push_back(0); // priority + nonce.insert(nonce.end(), sender.begin(), sender.end()); // Address 2 nonce.insert(nonce.end(), packet_number.begin(), packet_number.end()); // PN try { @@ -161,15 +168,17 @@ static std::vector<u8> DecryptDataFrame(const std::vector<u8>& encrypted_payload d.SetKeyWithIV(ccmp_key.data(), ccmp_key.size(), nonce.data(), nonce.size()); d.SpecifyDataLengths(aad.size(), encrypted_payload.size() - 8, 0); - CryptoPP::AuthenticatedDecryptionFilter df(d, nullptr, - CryptoPP::AuthenticatedDecryptionFilter::MAC_AT_END | - CryptoPP::AuthenticatedDecryptionFilter::THROW_EXCEPTION); + CryptoPP::AuthenticatedDecryptionFilter df( + d, nullptr, CryptoPP::AuthenticatedDecryptionFilter::MAC_AT_END | + CryptoPP::AuthenticatedDecryptionFilter::THROW_EXCEPTION); // put aad df.ChannelPut(CryptoPP::AAD_CHANNEL, aad.data(), aad.size()); // put cipher with mac - df.ChannelPut(CryptoPP::DEFAULT_CHANNEL, encrypted_payload.data(), encrypted_payload.size() - 8); - df.ChannelPut(CryptoPP::DEFAULT_CHANNEL, encrypted_payload.data() + encrypted_payload.size() - 8, 8); + df.ChannelPut(CryptoPP::DEFAULT_CHANNEL, encrypted_payload.data(), + encrypted_payload.size() - 8); + df.ChannelPut(CryptoPP::DEFAULT_CHANNEL, + encrypted_payload.data() + encrypted_payload.size() - 8, 8); df.ChannelMessageEnd(CryptoPP::AAD_CHANNEL); df.ChannelMessageEnd(CryptoPP::DEFAULT_CHANNEL); @@ -191,20 +200,25 @@ static std::vector<u8> DecryptDataFrame(const std::vector<u8>& encrypted_payload * Encrypts the payload of an 802.11 data frame using the specified key. * @returns The encrypted payload. */ -static std::vector<u8> EncryptDataFrame(const std::vector<u8>& payload, const std::array<u8, CryptoPP::AES::BLOCKSIZE>& ccmp_key, - const MacAddress& sender, const MacAddress& receiver, u16 sequence_number) { +static std::vector<u8> EncryptDataFrame(const std::vector<u8>& payload, + const std::array<u8, CryptoPP::AES::BLOCKSIZE>& ccmp_key, + const MacAddress& sender, const MacAddress& receiver, + u16 sequence_number) { // Reference: IEEE 802.11-2007 std::vector<u8> aad = GenerateCCMPAAD(sender, receiver); - std::vector<u8> packet_number{0, 0, 0, 0, - static_cast<u8>((sequence_number >> 8) & 0xFF), - static_cast<u8>(sequence_number & 0xFF)}; + std::vector<u8> packet_number{0, + 0, + 0, + 0, + static_cast<u8>((sequence_number >> 8) & 0xFF), + static_cast<u8>(sequence_number & 0xFF)}; // 8.3.3.3.3 Construct CCM nonce (13 bytes) std::vector<u8> nonce; - nonce.push_back(0); // priority - nonce.insert(nonce.end(), sender.begin(), sender.end()); // Address 2 + nonce.push_back(0); // priority + nonce.insert(nonce.end(), sender.begin(), sender.end()); // Address 2 nonce.insert(nonce.end(), packet_number.begin(), packet_number.end()); // PN try { @@ -235,11 +249,11 @@ static std::vector<u8> EncryptDataFrame(const std::vector<u8>& payload, const st return {}; } -std::vector<u8> GenerateDataPayload(const std::vector<u8>& data, u8 channel, u16 dest_node, u16 src_node, - u16 sequence_number) { +std::vector<u8> GenerateDataPayload(const std::vector<u8>& data, u8 channel, u16 dest_node, + u16 src_node, u16 sequence_number) { std::vector<u8> buffer = GenerateLLCHeader(EtherType::SecureData); - std::vector<u8> securedata_header = GenerateSecureDataHeader(data.size(), channel, dest_node, src_node, - sequence_number); + std::vector<u8> securedata_header = + GenerateSecureDataHeader(data.size(), channel, dest_node, src_node, sequence_number); buffer.insert(buffer.end(), securedata_header.begin(), securedata_header.end()); buffer.insert(buffer.end(), data.begin(), data.end()); |