From 7efb64132db95ef787481026a22d5b8244627a04 Mon Sep 17 00:00:00 2001 From: Subv Date: Tue, 13 Jun 2017 16:30:17 -0500 Subject: [PATCH 1/7] UDS: Stub SendTo to generate the unencrypted data frame with the right headers. --- src/core/CMakeLists.txt | 2 + src/core/hle/service/nwm/nwm_uds.cpp | 68 +++++++++++++++- src/core/hle/service/nwm/uds_data.cpp | 112 ++++++++++++++++++++++++++ src/core/hle/service/nwm/uds_data.h | 80 ++++++++++++++++++ 4 files changed, 261 insertions(+), 1 deletion(-) create mode 100644 src/core/hle/service/nwm/uds_data.cpp create mode 100644 src/core/hle/service/nwm/uds_data.h diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index b16a89990..ea09819e5 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -144,6 +144,7 @@ set(SRCS hle/service/nwm/nwm_tst.cpp hle/service/nwm/nwm_uds.cpp hle/service/nwm/uds_beacon.cpp + hle/service/nwm/uds_data.cpp hle/service/pm_app.cpp hle/service/ptm/ptm.cpp hle/service/ptm/ptm_gets.cpp @@ -341,6 +342,7 @@ set(HEADERS hle/service/nwm/nwm_tst.h hle/service/nwm/nwm_uds.h hle/service/nwm/uds_beacon.h + hle/service/nwm/uds_data.h hle/service/pm_app.h hle/service/ptm/ptm.h hle/service/ptm/ptm_gets.h diff --git a/src/core/hle/service/nwm/nwm_uds.cpp b/src/core/hle/service/nwm/nwm_uds.cpp index e92900d48..f6125825f 100644 --- a/src/core/hle/service/nwm/nwm_uds.cpp +++ b/src/core/hle/service/nwm/nwm_uds.cpp @@ -15,6 +15,7 @@ #include "core/hle/result.h" #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/memory.h" namespace Service { @@ -372,6 +373,71 @@ static void DestroyNetwork(Interface* self) { LOG_WARNING(Service_NWM, "called"); } +/** + * NWM_UDS::SendTo service function. + * Sends a data frame to the UDS network we're connected to. + * Inputs: + * 0 : Command header. + * 1 : Unknown. + * 2 : u16 Destination network node id. + * 3 : u8 Data channel. + * 4 : Buffer size >> 2 + * 5 : Data size + * 6 : Flags + * 7 : Input buffer descriptor + * 8 : Input buffer address + * Outputs: + * 0 : Return header + * 1 : Result of function, 0 on success, otherwise error code + */ +static void SendTo(Interface* self) { + IPC::RequestParser rp(Kernel::GetCommandBuffer(), 0x17, 6, 2); + + rp.Skip(1, false); + u16 dest_node_id = rp.Pop(); + u8 data_channel = rp.Pop(); + rp.Skip(1, false); + u32 data_size = rp.Pop(); + u32 flags = rp.Pop(); + + size_t desc_size; + const VAddr input_address = rp.PopStaticBuffer(&desc_size, false); + ASSERT(desc_size == data_size); + + // TODO(Subv): Figure out the error if this is called while not connected to a network. + if (connection_status.status == static_cast(NetworkStatus::ConnectedAsClient) || + connection_status.status == static_cast(NetworkStatus::ConnectedAsHost)) { + ASSERT_MSG(false, "Not connected to a network (unimplemented)"); + } + + // TODO(Subv): Do something with the flags. + + IPC::RequestBuilder rb = rp.MakeBuilder(1, 0); + + constexpr size_t MaxSize = 0x5C6; + if (data_size > MaxSize) { + rb.Push(ResultCode(ErrorDescription::TooLarge, ErrorModule::UDS, + ErrorSummary::WrongArgument, ErrorLevel::Usage)); + return; + } + + std::vector data(data_size); + Memory::ReadBlock(input_address, data.data(), data.size()); + + // TODO(Subv): Increment the sequence number after each sent packet. + u16 sequence_number = 0; + std::vector data_frame = GenerateDataFrame(data, data_channel, dest_node_id, + connection_status.network_node_id, + sequence_number); + + // TODO(Subv): Send the frame. + + rb.Push(RESULT_SUCCESS); + + LOG_WARNING(Service_NWM, "(STUB) called dest_node_id=%u size=%u flags=%u channel=%u", + static_cast(dest_node_id), data_size, flags, static_cast(data_channel)); +} + /** * NWM_UDS::GetChannel service function. * Returns the WiFi channel in which the network we're connected to is transmitting. @@ -564,7 +630,7 @@ const Interface::FunctionInfo FunctionTable[] = { {0x00130040, nullptr, "Unbind"}, {0x001400C0, nullptr, "PullPacket"}, {0x00150080, nullptr, "SetMaxSendDelay"}, - {0x00170182, nullptr, "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 new file mode 100644 index 000000000..7e4aec624 --- /dev/null +++ b/src/core/hle/service/nwm/uds_data.cpp @@ -0,0 +1,112 @@ +// Copyright 2017 Citra Emulator Project +// Licensed under GPLv2 or any later version +// Refer to the license.txt file included. + +#include + +#include "core/hle/service/nwm/nwm_uds.h" +#include "core/hle/service/nwm/uds_data.h" +#include "core/hw/aes/key.h" + +#include +#include +#include + +namespace Service { +namespace NWM { + +// AES Keyslot used to generate the UDS data frame CCMP key. +constexpr size_t UDSDataCryptoAESKeySlot = 0x2D; + +/* + * Generates a SNAP-enabled 802.2 LLC header for the specified protocol. + * @returns a buffer with the bytes of the generated header. + */ +static std::vector GenerateLLCHeader(EtherType protocol) { + LLCHeader header{}; + header.protocol = static_cast(protocol); + + std::vector buffer(sizeof(header)); + memcpy(buffer.data(), &header, sizeof(header)); + + return buffer; +} + +/* + * Generates a Nintendo UDS SecureData header with the specified parameters. + * @returns a buffer with the bytes of the generated header. + */ +static std::vector GenerateSecureDataHeader(u16 data_size, u8 channel, u16 dest_node_id, + u16 src_node_id, u16 sequence_number) { + SecureDataHeader header{}; + header.protocol_size = data_size + sizeof(SecureDataHeader); + // TODO(Subv): It is likely that the first 4 bytes of this header are actually a decorator for another protocol. + header.securedata_size = data_size + sizeof(SecureDataHeader) - 4; + header.is_management = 0; // Frames sent by the emulated application are never UDS management frames + header.data_channel = channel; + header.sequence_number = sequence_number; + header.dest_node_id = dest_node_id; + header.src_node_id = src_node_id; + + std::vector buffer(sizeof(header)); + memcpy(buffer.data(), &header, sizeof(header)); + + return buffer; +} + +/* + * Calculates the CTR used for the AES-CTR process that calculates + * the CCMP crypto key for data frames. + * @returns The CTR used for data frames crypto key generation. + */ +static std::array GetDataCryptoCTR(const NetworkInfo& network_info) { + DataFrameCryptoCTR data{}; + + data.host_mac = network_info.host_mac_address; + data.wlan_comm_id = network_info.wlan_comm_id; + data.id = network_info.id; + data.network_id = network_info.network_id; + + std::array hash; + CryptoPP::MD5().CalculateDigest(hash.data(), reinterpret_cast(&data), sizeof(data)); + + return hash; +} + +/* + * 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 GenerateDataCCMPKey(const std::vector& passphrase, + const NetworkInfo& network_info) { + // Calculate the MD5 hash of the input passphrase. + std::array passphrase_hash; + CryptoPP::MD5().CalculateDigest(passphrase_hash.data(), passphrase.data(), passphrase.size()); + + std::array ccmp_key; + + // 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 counter = GetDataCryptoCTR(network_info); + std::array key = HW::AES::GetNormalKey(UDSDataCryptoAESKeySlot); + CryptoPP::CTR_Mode::Encryption aes; + aes.SetKeyWithIV(key.data(), AES::BLOCKSIZE, counter.data()); + aes.ProcessData(ccmp_key.data(), passphrase_hash.data(), passphrase_hash.size()); + + return ccmp_key; +} + +std::vector GenerateDataFrame(const std::vector& data, u8 channel, u16 dest_node, u16 src_node, u16 sequence_number) { + std::vector buffer = GenerateLLCHeader(EtherType::SecureData); + std::vector 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()); + // TODO(Subv): Encrypt the frame. + // TODO(Subv): Prepend CCMP initialization vector (sequence_number). + // TODO(Subv): Encapsulate the frame in an 802.11 data frame. + return buffer; +} + +} // namespace NWM +} // namespace Service diff --git a/src/core/hle/service/nwm/uds_data.h b/src/core/hle/service/nwm/uds_data.h new file mode 100644 index 000000000..0dd46bcb1 --- /dev/null +++ b/src/core/hle/service/nwm/uds_data.h @@ -0,0 +1,80 @@ +// Copyright 2017 Citra Emulator Project +// Licensed under GPLv2 or any later version +// Refer to the license.txt file included. + +#pragma once + +#include "common/common_types.h" +#include "common/swap.h" +#include "core/hle/service/service.h" + +namespace Service { +namespace NWM { + +enum class SAP : u8 { + SNAPExtensionUsed = 0xAA +}; + +enum class PDUControl : u8 { + UnnumberedInformation = 3 +}; + +enum class EtherType : u16 { + SecureData = 0x876D, + EAPoL = 0x888E +}; + +/* + * 802.2 header, UDS packets always use SNAP for these headers, + * which means the dsap and ssap are always SNAPExtensionUsed (0xAA) + * and the OUI is always 0. + */ +struct LLCHeader { + u8 dsap = static_cast(SAP::SNAPExtensionUsed); + u8 ssap = static_cast(SAP::SNAPExtensionUsed); + u8 control = static_cast(PDUControl::UnnumberedInformation); + std::array OUI = {}; + u16_be protocol; +}; + +static_assert(sizeof(LLCHeader) == 8, "LLCHeader has the wrong size"); + +/* + * Nintendo SecureData header, every UDS packet contains one, + * it is used to store metadata about the transmission such as + * the source and destination network node ids. + */ +struct SecureDataHeader { + u16_be protocol_size; + INSERT_PADDING_BYTES(2); + u16_be securedata_size; + u8 is_management; + u8 data_channel; + u16_be sequence_number; + u16_be dest_node_id; + u16_be src_node_id; +}; + +static_assert(sizeof(SecureDataHeader) == 14, "SecureDataHeader has the wrong size"); + +/* + * The raw bytes of this structure are the CTR used in the encryption (AES-CTR) + * process used to generate the CCMP key for data frame encryption. + */ +struct DataFrameCryptoCTR { + u32_le wlan_comm_id; + u32_le network_id; + std::array host_mac; + u16_le id; +}; + +static_assert(sizeof(DataFrameCryptoCTR) == 16, "DataFrameCryptoCTR has the wrong size"); + +/** + * Generates an encrypted 802.11 data frame starting at the CCMP IV. + * @returns The generated frame. + */ +std::vector GenerateDataFrame(const std::vector& data, u8 channel, u16 dest_node, u16 src_node, u16 sequence_number); + +} // namespace NWM +} // namespace Service From 61ce89a55ac6ff12f881e3bba0220ac3f04fbf50 Mon Sep 17 00:00:00 2001 From: Subv Date: Tue, 13 Jun 2017 21:50:22 -0500 Subject: [PATCH 2/7] UDS: Return the correct error messages in SendTo when not connected to a network or trying to send to itself. --- src/core/hle/service/nwm/nwm_uds.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/core/hle/service/nwm/nwm_uds.cpp b/src/core/hle/service/nwm/nwm_uds.cpp index f6125825f..c43c5ca44 100644 --- a/src/core/hle/service/nwm/nwm_uds.cpp +++ b/src/core/hle/service/nwm/nwm_uds.cpp @@ -404,16 +404,23 @@ static void SendTo(Interface* self) { const VAddr input_address = rp.PopStaticBuffer(&desc_size, false); ASSERT(desc_size == data_size); - // TODO(Subv): Figure out the error if this is called while not connected to a network. - if (connection_status.status == static_cast(NetworkStatus::ConnectedAsClient) || - connection_status.status == static_cast(NetworkStatus::ConnectedAsHost)) { - ASSERT_MSG(false, "Not connected to a network (unimplemented)"); + IPC::RequestBuilder rb = rp.MakeBuilder(1, 0); + + if (connection_status.status != static_cast(NetworkStatus::ConnectedAsClient) && + connection_status.status != static_cast(NetworkStatus::ConnectedAsHost)) { + rb.Push(ResultCode(ErrorDescription::NotAuthorized, ErrorModule::UDS, + ErrorSummary::InvalidState, ErrorLevel::Status)); + return; + } + + if (dest_node_id == connection_status.network_node_id) { + rb.Push(ResultCode(ErrorDescription::NotFound, ErrorModule::UDS, + ErrorSummary::WrongArgument, ErrorLevel::Status)); + return; } // TODO(Subv): Do something with the flags. - IPC::RequestBuilder rb = rp.MakeBuilder(1, 0); - constexpr size_t MaxSize = 0x5C6; if (data_size > MaxSize) { rb.Push(ResultCode(ErrorDescription::TooLarge, ErrorModule::UDS, From 812b404492d36d499b4a74019eacb6da2a052a26 Mon Sep 17 00:00:00 2001 From: Subv Date: Wed, 14 Jun 2017 09:43:05 -0500 Subject: [PATCH 3/7] UDS: Clarify comment about the first 4 bytes of the SecureData header. It is likely that these 4 bytes are actually a different header, part of some protocol that encapsulates the SecureData protocol. --- src/core/hle/service/nwm/uds_data.cpp | 4 +++- src/core/hle/service/nwm/uds_data.h | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/core/hle/service/nwm/uds_data.cpp b/src/core/hle/service/nwm/uds_data.cpp index 7e4aec624..9ba2fdcf1 100644 --- a/src/core/hle/service/nwm/uds_data.cpp +++ b/src/core/hle/service/nwm/uds_data.cpp @@ -40,7 +40,9 @@ static std::vector GenerateSecureDataHeader(u16 data_size, u8 channel, u16 d u16 src_node_id, u16 sequence_number) { SecureDataHeader header{}; header.protocol_size = data_size + sizeof(SecureDataHeader); - // TODO(Subv): It is likely that the first 4 bytes of this header are actually a decorator for another protocol. + // 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 header.data_channel = channel; diff --git a/src/core/hle/service/nwm/uds_data.h b/src/core/hle/service/nwm/uds_data.h index 0dd46bcb1..8480ef94b 100644 --- a/src/core/hle/service/nwm/uds_data.h +++ b/src/core/hle/service/nwm/uds_data.h @@ -45,6 +45,8 @@ static_assert(sizeof(LLCHeader) == 8, "LLCHeader has the wrong size"); * the source and destination network node ids. */ struct SecureDataHeader { + // TODO(Subv): It is likely that the first 4 bytes of this header are + // actually part of another container protocol. u16_be protocol_size; INSERT_PADDING_BYTES(2); u16_be securedata_size; From 9befb8c887b78128a2e8ef8febc82a4933196602 Mon Sep 17 00:00:00 2001 From: Subv Date: Wed, 14 Jun 2017 12:47:52 -0500 Subject: [PATCH 4/7] UDS: Added functions to encrypt and decrypt the data frames. The responsibility of encryption and encapsulation into an 802.11 MAC frame will fall into the callers of GenerateDataPayload. --- src/core/hle/service/nwm/nwm_uds.cpp | 9 +- src/core/hle/service/nwm/uds_data.cpp | 148 ++++++++++++++++++++++++-- src/core/hle/service/nwm/uds_data.h | 11 +- 3 files changed, 156 insertions(+), 12 deletions(-) diff --git a/src/core/hle/service/nwm/nwm_uds.cpp b/src/core/hle/service/nwm/nwm_uds.cpp index c43c5ca44..d9bd9c4a4 100644 --- a/src/core/hle/service/nwm/nwm_uds.cpp +++ b/src/core/hle/service/nwm/nwm_uds.cpp @@ -433,9 +433,12 @@ static void SendTo(Interface* self) { // TODO(Subv): Increment the sequence number after each sent packet. u16 sequence_number = 0; - std::vector data_frame = GenerateDataFrame(data, data_channel, dest_node_id, - connection_status.network_node_id, - sequence_number); + std::vector 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. // TODO(Subv): Send the frame. diff --git a/src/core/hle/service/nwm/uds_data.cpp b/src/core/hle/service/nwm/uds_data.cpp index 9ba2fdcf1..e05ca8815 100644 --- a/src/core/hle/service/nwm/uds_data.cpp +++ b/src/core/hle/service/nwm/uds_data.cpp @@ -5,10 +5,12 @@ #include #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 +#include +#include #include #include @@ -98,15 +100,149 @@ static std::array GenerateDataCCMPKey(const std::v return ccmp_key; } -std::vector GenerateDataFrame(const std::vector& data, u8 channel, u16 dest_node, u16 src_node, u16 sequence_number) { +/* + * Generates the Additional Authenticated Data (AAD) for an UDS 802.11 encrypted data frame. + * @returns a buffer with the bytes of the AAD. + */ +static std::vector GenerateCCMPAAD(const MacAddress& sender, const MacAddress& receiver) { + // Reference: IEEE 802.11-2007 + + // 8.3.3.3.2 Construct AAD (22-30 bytes) + // The AAD is constructed from the MPDU header. The AAD does not include the header Duration + // field, because the Duration field value can change due to normal IEEE 802.11 operation (e.g., + // a rate change during retransmission). For similar reasons, several subfields in the Frame + // Control field are masked to 0. + struct { + u16_be FC; // MPDU Frame Control field + MacAddress receiver; + MacAddress transmitter; + MacAddress destination; + u16_be SC; // MPDU Sequence Control field + } aad_struct{}; + + // Default FC value of DataFrame | Protected | ToDS + constexpr u16 DefaultFrameControl = 0x0841; + + aad_struct.FC = DefaultFrameControl; + aad_struct.SC = 0; + aad_struct.transmitter = sender; + aad_struct.receiver = receiver; + aad_struct.destination = receiver; + + std::vector aad(sizeof(aad_struct)); + std::memcpy(aad.data(), &aad_struct, sizeof(aad_struct)); + + return aad; +} + +/* + * Decrypts the payload of an encrypted 802.11 data frame using the specified key. + * @returns The decrypted payload. + */ +static std::vector DecryptDataFrame(const std::vector& encrypted_payload, const std::array& ccmp_key, + const MacAddress& sender, const MacAddress& receiver, u16 sequence_number) { + + // Reference: IEEE 802.11-2007 + + std::vector aad = GenerateCCMPAAD(sender, receiver); + + std::vector packet_number{0, 0, 0, 0, + static_cast((sequence_number >> 8) & 0xFF), + static_cast(sequence_number & 0xFF)}; + + // 8.3.3.3.3 Construct CCM nonce (13 bytes) + std::vector nonce; + 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 { + CryptoPP::CCM::Decryption d; + 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); + // 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.ChannelMessageEnd(CryptoPP::AAD_CHANNEL); + df.ChannelMessageEnd(CryptoPP::DEFAULT_CHANNEL); + df.SetRetrievalChannel(CryptoPP::DEFAULT_CHANNEL); + + int size = df.MaxRetrievable(); + + std::vector pdata(size); + df.Get(pdata.data(), size); + return pdata; + } catch (CryptoPP::Exception&) { + LOG_ERROR(Service_NWM, "failed to decrypt"); + } + + return {}; +} + +/* + * Encrypts the payload of an 802.11 data frame using the specified key. + * @returns The encrypted payload. + */ +static std::vector EncryptDataFrame(const std::vector& payload, const std::array& ccmp_key, + const MacAddress& sender, const MacAddress& receiver, u16 sequence_number) { + // Reference: IEEE 802.11-2007 + + std::vector aad = GenerateCCMPAAD(sender, receiver); + + std::vector packet_number{0, 0, 0, 0, + static_cast((sequence_number >> 8) & 0xFF), + static_cast(sequence_number & 0xFF)}; + + // 8.3.3.3.3 Construct CCM nonce (13 bytes) + std::vector nonce; + 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 { + CryptoPP::CCM::Encryption d; + d.SetKeyWithIV(ccmp_key.data(), ccmp_key.size(), nonce.data(), nonce.size()); + d.SpecifyDataLengths(aad.size(), payload.size(), 0); + + CryptoPP::AuthenticatedEncryptionFilter df(d); + // put aad + df.ChannelPut(CryptoPP::AAD_CHANNEL, aad.data(), aad.size()); + df.ChannelMessageEnd(CryptoPP::AAD_CHANNEL); + + // put plaintext + df.ChannelPut(CryptoPP::DEFAULT_CHANNEL, payload.data(), payload.size()); + df.ChannelMessageEnd(CryptoPP::DEFAULT_CHANNEL); + + df.SetRetrievalChannel(CryptoPP::DEFAULT_CHANNEL); + + int size = df.MaxRetrievable(); + + std::vector cipher(size); + df.Get(cipher.data(), size); + return cipher; + } catch (CryptoPP::Exception&) { + LOG_ERROR(Service_NWM, "failed to encrypt"); + } + + return {}; +} + +std::vector GenerateDataPayload(const std::vector& data, u8 channel, u16 dest_node, u16 src_node, + u16 sequence_number) { std::vector buffer = GenerateLLCHeader(EtherType::SecureData); - std::vector securedata_header = GenerateSecureDataHeader(data.size(), channel, dest_node, src_node, sequence_number); + std::vector 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()); - // TODO(Subv): Encrypt the frame. - // TODO(Subv): Prepend CCMP initialization vector (sequence_number). - // TODO(Subv): Encapsulate the frame in an 802.11 data frame. return buffer; } diff --git a/src/core/hle/service/nwm/uds_data.h b/src/core/hle/service/nwm/uds_data.h index 8480ef94b..960f13cee 100644 --- a/src/core/hle/service/nwm/uds_data.h +++ b/src/core/hle/service/nwm/uds_data.h @@ -4,10 +4,15 @@ #pragma once +#include +#include + #include "common/common_types.h" #include "common/swap.h" #include "core/hle/service/service.h" +#include + namespace Service { namespace NWM { @@ -73,10 +78,10 @@ struct DataFrameCryptoCTR { static_assert(sizeof(DataFrameCryptoCTR) == 16, "DataFrameCryptoCTR has the wrong size"); /** - * Generates an encrypted 802.11 data frame starting at the CCMP IV. - * @returns The generated frame. + * Generates an unencrypted 802.11 data payload. + * @returns The generated frame payload. */ -std::vector GenerateDataFrame(const std::vector& data, u8 channel, u16 dest_node, u16 src_node, u16 sequence_number); +std::vector GenerateDataPayload(const std::vector& data, u8 channel, u16 dest_node, u16 src_node, u16 sequence_number); } // namespace NWM } // namespace Service From 87168bfe8b5b0b96f7e39f33db1df52da046c39a Mon Sep 17 00:00:00 2001 From: Subv Date: Wed, 14 Jun 2017 13:18:58 -0500 Subject: [PATCH 5/7] UDS: Run clang-format. --- src/core/hle/service/nwm/nwm_uds.cpp | 7 +-- src/core/hle/service/nwm/uds_data.cpp | 80 ++++++++++++++++----------- 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 data_payload = GenerateDataPayload(data, data_channel, dest_node_id, - connection_status.network_node_id, - sequence_number); + std::vector 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 - -#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 #include #include #include #include +#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; + // AES Keyslot used to generate the UDS data frame CCMP key. constexpr size_t UDSDataCryptoAESKeySlot = 0x2D; @@ -39,14 +39,15 @@ static std::vector GenerateLLCHeader(EtherType protocol) { * @returns a buffer with the bytes of the generated header. */ static std::vector 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 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 GetDataCryptoCTR(const NetworkInfo& network_info) { @@ -81,15 +82,16 @@ static std::array 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 GenerateDataCCMPKey(const std::vector& passphrase, - const NetworkInfo& network_info) { +static std::array GenerateDataCCMPKey( + const std::vector& passphrase, const NetworkInfo& network_info) { // Calculate the MD5 hash of the input passphrase. std::array passphrase_hash; CryptoPP::MD5().CalculateDigest(passphrase_hash.data(), passphrase.data(), passphrase.size()); std::array 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 counter = GetDataCryptoCTR(network_info); std::array key = HW::AES::GetNormalKey(UDSDataCryptoAESKeySlot); @@ -139,21 +141,26 @@ static std::vector 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 DecryptDataFrame(const std::vector& encrypted_payload, const std::array& ccmp_key, - const MacAddress& sender, const MacAddress& receiver, u16 sequence_number) { +static std::vector DecryptDataFrame(const std::vector& encrypted_payload, + const std::array& ccmp_key, + const MacAddress& sender, const MacAddress& receiver, + u16 sequence_number) { // Reference: IEEE 802.11-2007 std::vector aad = GenerateCCMPAAD(sender, receiver); - std::vector packet_number{0, 0, 0, 0, + std::vector packet_number{0, + 0, + 0, + 0, static_cast((sequence_number >> 8) & 0xFF), static_cast(sequence_number & 0xFF)}; // 8.3.3.3.3 Construct CCM nonce (13 bytes) std::vector 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 DecryptDataFrame(const std::vector& 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 DecryptDataFrame(const std::vector& encrypted_payload * Encrypts the payload of an 802.11 data frame using the specified key. * @returns The encrypted payload. */ -static std::vector EncryptDataFrame(const std::vector& payload, const std::array& ccmp_key, - const MacAddress& sender, const MacAddress& receiver, u16 sequence_number) { +static std::vector EncryptDataFrame(const std::vector& payload, + const std::array& ccmp_key, + const MacAddress& sender, const MacAddress& receiver, + u16 sequence_number) { // Reference: IEEE 802.11-2007 std::vector aad = GenerateCCMPAAD(sender, receiver); - std::vector packet_number{0, 0, 0, 0, - static_cast((sequence_number >> 8) & 0xFF), - static_cast(sequence_number & 0xFF)}; + std::vector packet_number{0, + 0, + 0, + 0, + static_cast((sequence_number >> 8) & 0xFF), + static_cast(sequence_number & 0xFF)}; // 8.3.3.3.3 Construct CCM nonce (13 bytes) std::vector 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 EncryptDataFrame(const std::vector& payload, const st return {}; } -std::vector GenerateDataPayload(const std::vector& data, u8 channel, u16 dest_node, u16 src_node, - u16 sequence_number) { +std::vector GenerateDataPayload(const std::vector& data, u8 channel, u16 dest_node, + u16 src_node, u16 sequence_number) { std::vector buffer = GenerateLLCHeader(EtherType::SecureData); - std::vector securedata_header = GenerateSecureDataHeader(data.size(), channel, dest_node, src_node, - sequence_number); + std::vector 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 #include - #include "common/common_types.h" #include "common/swap.h" #include "core/hle/service/service.h" -#include - 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 GenerateDataPayload(const std::vector& data, u8 channel, u16 dest_node, u16 src_node, u16 sequence_number); +std::vector GenerateDataPayload(const std::vector& data, u8 channel, u16 dest_node, + u16 src_node, u16 sequence_number); } // namespace NWM } // namespace Service From 1f1739d366baf7d0a0cf0a93c43987e1031387bf Mon Sep 17 00:00:00 2001 From: Subv Date: Wed, 14 Jun 2017 14:21:35 -0500 Subject: [PATCH 6/7] UDS: Move the UDS keyslot used to generate the CCMP key to the AES::KeySlotID enum. --- src/core/hle/service/nwm/uds_data.cpp | 5 +---- src/core/hw/aes/key.h | 2 ++ 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/core/hle/service/nwm/uds_data.cpp b/src/core/hle/service/nwm/uds_data.cpp index fabdf67a8..280c73e05 100644 --- a/src/core/hle/service/nwm/uds_data.cpp +++ b/src/core/hle/service/nwm/uds_data.cpp @@ -17,9 +17,6 @@ namespace NWM { using MacAddress = std::array; -// AES Keyslot used to generate the UDS data frame CCMP key. -constexpr size_t UDSDataCryptoAESKeySlot = 0x2D; - /* * Generates a SNAP-enabled 802.2 LLC header for the specified protocol. * @returns a buffer with the bytes of the generated header. @@ -94,7 +91,7 @@ static std::array GenerateDataCCMPKey( // keyslot 0x2D. using CryptoPP::AES; std::array counter = GetDataCryptoCTR(network_info); - std::array key = HW::AES::GetNormalKey(UDSDataCryptoAESKeySlot); + std::array key = HW::AES::GetNormalKey(HW::AES::KeySlotID::UDSDataKey); CryptoPP::CTR_Mode::Encryption aes; aes.SetKeyWithIV(key.data(), AES::BLOCKSIZE, counter.data()); aes.ProcessData(ccmp_key.data(), passphrase_hash.data(), passphrase_hash.size()); diff --git a/src/core/hw/aes/key.h b/src/core/hw/aes/key.h index b01d04f13..c9f1342f4 100644 --- a/src/core/hw/aes/key.h +++ b/src/core/hw/aes/key.h @@ -12,6 +12,8 @@ namespace HW { namespace AES { enum KeySlotID : size_t { + // AES Keyslot used to generate the UDS data frame CCMP key. + UDSDataKey = 0x2D, APTWrap = 0x31, MaxKeySlotID = 0x40, From 2eb174713b8f819f7e013e48efb2d0736c065ed9 Mon Sep 17 00:00:00 2001 From: Subv Date: Wed, 14 Jun 2017 16:59:16 -0500 Subject: [PATCH 7/7] UDS: Use the ToDS and FromDS fields to properly calculate the AAD used during encryption. --- src/core/hle/service/nwm/uds_data.cpp | 47 ++++++++++++++++++--------- 1 file changed, 32 insertions(+), 15 deletions(-) diff --git a/src/core/hle/service/nwm/uds_data.cpp b/src/core/hle/service/nwm/uds_data.cpp index 280c73e05..8c6742dba 100644 --- a/src/core/hle/service/nwm/uds_data.cpp +++ b/src/core/hle/service/nwm/uds_data.cpp @@ -103,7 +103,8 @@ static std::array GenerateDataCCMPKey( * Generates the Additional Authenticated Data (AAD) for an UDS 802.11 encrypted data frame. * @returns a buffer with the bytes of the AAD. */ -static std::vector GenerateCCMPAAD(const MacAddress& sender, const MacAddress& receiver) { +static std::vector GenerateCCMPAAD(const MacAddress& sender, const MacAddress& receiver, + const MacAddress& bssid, u16 frame_control) { // Reference: IEEE 802.11-2007 // 8.3.3.3.2 Construct AAD (22-30 bytes) @@ -113,20 +114,34 @@ static std::vector GenerateCCMPAAD(const MacAddress& sender, const MacAddres // Control field are masked to 0. struct { u16_be FC; // MPDU Frame Control field - MacAddress receiver; - MacAddress transmitter; - MacAddress destination; + MacAddress A1; + MacAddress A2; + MacAddress A3; u16_be SC; // MPDU Sequence Control field } aad_struct{}; - // Default FC value of DataFrame | Protected | ToDS - constexpr u16 DefaultFrameControl = 0x0841; - - aad_struct.FC = DefaultFrameControl; + constexpr u16 AADFrameControlMask = 0x8FC7; + aad_struct.FC = frame_control & AADFrameControlMask; aad_struct.SC = 0; - aad_struct.transmitter = sender; - aad_struct.receiver = receiver; - aad_struct.destination = receiver; + + bool to_ds = (frame_control & (1 << 0)) != 0; + bool from_ds = (frame_control & (1 << 1)) != 0; + // In the 802.11 standard, ToDS = 1 and FromDS = 1 is a valid configuration, + // however, the 3DS doesn't seem to transmit frames with such combination. + ASSERT_MSG(to_ds != from_ds, "Invalid combination"); + + // The meaning of the address fields depends on the ToDS and FromDS fields. + if (from_ds) { + aad_struct.A1 = receiver; + aad_struct.A2 = bssid; + aad_struct.A3 = sender; + } + + if (to_ds) { + aad_struct.A1 = bssid; + aad_struct.A2 = sender; + aad_struct.A3 = receiver; + } std::vector aad(sizeof(aad_struct)); std::memcpy(aad.data(), &aad_struct, sizeof(aad_struct)); @@ -141,11 +156,12 @@ static std::vector GenerateCCMPAAD(const MacAddress& sender, const MacAddres static std::vector DecryptDataFrame(const std::vector& encrypted_payload, const std::array& ccmp_key, const MacAddress& sender, const MacAddress& receiver, - u16 sequence_number) { + const MacAddress& bssid, u16 sequence_number, + u16 frame_control) { // Reference: IEEE 802.11-2007 - std::vector aad = GenerateCCMPAAD(sender, receiver); + std::vector aad = GenerateCCMPAAD(sender, receiver, bssid, frame_control); std::vector packet_number{0, 0, @@ -200,10 +216,11 @@ static std::vector DecryptDataFrame(const std::vector& encrypted_payload static std::vector EncryptDataFrame(const std::vector& payload, const std::array& ccmp_key, const MacAddress& sender, const MacAddress& receiver, - u16 sequence_number) { + const MacAddress& bssid, u16 sequence_number, + u16 frame_control) { // Reference: IEEE 802.11-2007 - std::vector aad = GenerateCCMPAAD(sender, receiver); + std::vector aad = GenerateCCMPAAD(sender, receiver, bssid, frame_control); std::vector packet_number{0, 0,