diff options
| -rw-r--r-- | src/core/hle/service/nwm/nwm_uds.cpp | 7 | ||||
| -rw-r--r-- | src/core/hle/service/nwm/uds_data.cpp | 80 | ||||
| -rw-r--r-- | src/core/hle/service/nwm/uds_data.h | 19 |
3 files changed, 55 insertions, 51 deletions
diff --git a/src/core/hle/service/nwm/nwm_uds.cpp b/src/core/hle/service/nwm/nwm_uds.cpp index d9bd9c4a4..35fa1cd77 100644 --- a/src/core/hle/service/nwm/nwm_uds.cpp +++ b/src/core/hle/service/nwm/nwm_uds.cpp @@ -433,9 +433,8 @@ static void SendTo(Interface* self) { // TODO(Subv): Increment the sequence number after each sent packet. u16 sequence_number = 0; - std::vector<u8> data_payload = GenerateDataPayload(data, data_channel, dest_node_id, - connection_status.network_node_id, - sequence_number); + std::vector<u8> data_payload = GenerateDataPayload( + data, data_channel, dest_node_id, connection_status.network_node_id, sequence_number); // TODO(Subv): Retrieve the MAC address of the dest_node_id and our own to encrypt // and encapsulate the payload. @@ -640,7 +639,7 @@ const Interface::FunctionInfo FunctionTable[] = { {0x00130040, nullptr, "Unbind"}, {0x001400C0, nullptr, "PullPacket"}, {0x00150080, nullptr, "SetMaxSendDelay"}, - {0x00170182, SendTo, "SendTo"}, + {0x00170182, SendTo, "SendTo"}, {0x001A0000, GetChannel, "GetChannel"}, {0x001B0302, InitializeWithVersion, "InitializeWithVersion"}, {0x001D0044, BeginHostingNetwork, "BeginHostingNetwork"}, 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()); diff --git a/src/core/hle/service/nwm/uds_data.h b/src/core/hle/service/nwm/uds_data.h index 960f13cee..a23520a41 100644 --- a/src/core/hle/service/nwm/uds_data.h +++ b/src/core/hle/service/nwm/uds_data.h @@ -6,28 +6,18 @@ #include <array> #include <vector> - #include "common/common_types.h" #include "common/swap.h" #include "core/hle/service/service.h" -#include <cryptopp/aes.h> - namespace Service { namespace NWM { -enum class SAP : u8 { - SNAPExtensionUsed = 0xAA -}; +enum class SAP : u8 { SNAPExtensionUsed = 0xAA }; -enum class PDUControl : u8 { - UnnumberedInformation = 3 -}; +enum class PDUControl : u8 { UnnumberedInformation = 3 }; -enum class EtherType : u16 { - SecureData = 0x876D, - EAPoL = 0x888E -}; +enum class EtherType : u16 { SecureData = 0x876D, EAPoL = 0x888E }; /* * 802.2 header, UDS packets always use SNAP for these headers, @@ -81,7 +71,8 @@ static_assert(sizeof(DataFrameCryptoCTR) == 16, "DataFrameCryptoCTR has the wron * Generates an unencrypted 802.11 data payload. * @returns The generated frame payload. */ -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); } // namespace NWM } // namespace Service |