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.
This commit is contained in:
Subv
parent
812b404492
commit
9befb8c887
|
|
@ -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<u8> data_frame = GenerateDataFrame(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.
|
||||
|
||||
// TODO(Subv): Send the frame.
|
||||
|
||||
|
|
|
|||
|
|
@ -5,10 +5,12 @@
|
|||
#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>
|
||||
|
||||
|
|
@ -98,15 +100,149 @@ static std::array<u8, CryptoPP::AES::BLOCKSIZE> GenerateDataCCMPKey(const std::v
|
|||
return ccmp_key;
|
||||
}
|
||||
|
||||
std::vector<u8> GenerateDataFrame(const std::vector<u8>& 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<u8> 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<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, 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)};
|
||||
|
||||
// 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, 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)};
|
||||
|
||||
// 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);
|
||||
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());
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -4,10 +4,15 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#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 {
|
||||
|
||||
|
|
@ -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<u8> GenerateDataFrame(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
|
||||
|
|
|
|||
Loading…
Reference in a new issue