1 files changed, 278 insertions, 0 deletions
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..8c6742dba
--- /dev/null
+++ b/src/core/hle/service/nwm/uds_data.cpp
@@ -0,0 +1,278 @@
+// Copyright 2017 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include <cstring>
+#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>;
+
+/*
+ * 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<u8> GenerateLLCHeader(EtherType protocol) {
+    LLCHeader header{};
+    header.protocol = static_cast<u16>(protocol);
+
+    std::vector<u8> 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<u8> 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);
+    // 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;
+    // 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;
+    header.src_node_id = src_node_id;
+
+    std::vector<u8> 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<u8, CryptoPP::MD5::DIGESTSIZE> 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<u8, CryptoPP::MD5::DIGESTSIZE> hash;
+    CryptoPP::MD5().CalculateDigest(hash.data(), reinterpret_cast<u8*>(&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<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.
+    using CryptoPP::AES;
+    std::array<u8, CryptoPP::MD5::DIGESTSIZE> counter = GetDataCryptoCTR(network_info);
+    std::array<u8, AES::BLOCKSIZE> key = HW::AES::GetNormalKey(HW::AES::KeySlotID::UDSDataKey);
+    CryptoPP::CTR_Mode<AES>::Encryption aes;
+    aes.SetKeyWithIV(key.data(), AES::BLOCKSIZE, counter.data());
+    aes.ProcessData(ccmp_key.data(), passphrase_hash.data(), passphrase_hash.size());
+
+    return ccmp_key;
+}
+
+/*
+ * 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<u8> 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)
+    // 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 A1;
+        MacAddress A2;
+        MacAddress A3;
+        u16_be SC; // MPDU Sequence Control field
+    } aad_struct{};
+
+    constexpr u16 AADFrameControlMask = 0x8FC7;
+    aad_struct.FC = frame_control & AADFrameControlMask;
+    aad_struct.SC = 0;
+
+    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<u8> 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<u8> DecryptDataFrame(const std::vector<u8>& encrypted_payload,
+                                        const std::array<u8, CryptoPP::AES::BLOCKSIZE>& ccmp_key,
+                                        const MacAddress& sender, const MacAddress& receiver,
+                                        const MacAddress& bssid, u16 sequence_number,
+                                        u16 frame_control) {
+
+    // Reference: IEEE 802.11-2007
+
+    std::vector<u8> aad = GenerateCCMPAAD(sender, receiver, bssid, frame_control);
+
+    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.insert(nonce.end(), packet_number.begin(), packet_number.end()); // PN
+
+    try {
+        CryptoPP::CCM<CryptoPP::AES, 8>::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<u8> 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<u8> EncryptDataFrame(const std::vector<u8>& payload,
+                                        const std::array<u8, CryptoPP::AES::BLOCKSIZE>& ccmp_key,
+                                        const MacAddress& sender, const MacAddress& receiver,
+                                        const MacAddress& bssid, u16 sequence_number,
+                                        u16 frame_control) {
+    // Reference: IEEE 802.11-2007
+
+    std::vector<u8> aad = GenerateCCMPAAD(sender, receiver, bssid, frame_control);
+
+    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.insert(nonce.end(), packet_number.begin(), packet_number.end()); // PN
+
+    try {
+        CryptoPP::CCM<CryptoPP::AES, 8>::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<u8> cipher(size);
+        df.Get(cipher.data(), size);
+        return cipher;
+    } catch (CryptoPP::Exception&) {
+        LOG_ERROR(Service_NWM, "failed to encrypt");
+    }
+
+    return {};
+}
+
+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);
+
+    buffer.insert(buffer.end(), securedata_header.begin(), securedata_header.end());
+    buffer.insert(buffer.end(), data.begin(), data.end());
+    return buffer;
+}
+
+} // namespace NWM
+} // namespace Service