//-----------------------------------------------------------------------------
// <Security::EncryptMessage>
// Encrypt and send a Z-Wave message securely.
//-----------------------------------------------------------------------------
bool Security::EncryptMessage
(
uint8 const* _nonce
)
{
#if 1
if( m_nonceTimer.GetMilliseconds() > 10000 )
{
// The nonce was not received within 10 seconds
// of us sending the nonce request. Send it again
RequestNonce();
return false;
}
// Fetch the next payload from the queue and encapsulate it
m_queueMutex->Lock();
if( m_queue.empty() )
{
// Nothing to do
m_queueMutex->Release();
return false;
}
SecurityPayload * payload = m_queue.front();
m_queue.pop_front();
//uint32 queueSize = m_queue.size();
m_queueMutex->Unlock();
#else
uint32 queueSize = m_queue.size();
struct SecurityPayload payload;
payload.m_length = 7;
payload.m_part = 0;
uint8 tmpdata[7] = {0x62, 0x03, 0x00, 0x10, 0x02, 0xfe, 0xfe};
for (int i = 0; i < payload.m_length; i++)
payload.m_data[i] = tmpdata[i];
#endif
// Encapsulate the message fragment
/* MessageEncapNonceGet doesn't seem to work */
//Msg* msg = new Msg( (queueSize>1) ? "SecurityCmd_MessageEncapNonceGet" : "SecurityCmd_MessageEncap", GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true );
string LogMessage("SecurityCmd_MessageEncap (");
LogMessage.append(payload->logmsg);
LogMessage.append(")");
Msg* msg = new Msg( LogMessage, GetNodeId(), REQUEST, FUNC_ID_ZW_SEND_DATA, true, true, FUNC_ID_APPLICATION_COMMAND_HANDLER, GetCommandClassId() );
msg->Append( GetNodeId() );
msg->Append( payload->m_length + 20 );
msg->Append( GetCommandClassId() );
//msg->Append( (queueSize>1) ? SecurityCmd_MessageEncapNonceGet : SecurityCmd_MessageEncap );
msg->Append( SecurityCmd_MessageEncap );
/* create the iv
*
*/
uint8 initializationVector[16];
/* the first 8 bytes of a outgoing IV are random */
for (int i = 0; i < 8; i++) {
//initializationVector[i] = (rand()%0xFF)+1;
initializationVector[i] = 0xAA;
}
/* the remaining 8 bytes are the NONCE we got from the device */
for (int i = 0; i < 8; i++) {
initializationVector[8+i] = _nonce[i];
}
/* Append the first 8 bytes of the initialization vector
* to the message. The remaining 8 bytes are the NONCE we recieved from
* the node, and is ommitted from sending back to the Node. But we use the full 16 bytes to
* as the IV to encrypt out message.
*/
for(int i=0; i<8; ++i )
{
msg->Append( initializationVector[i] );
}
// Append the sequence data
uint8 sequence = 0;
if( payload->m_part == 0 )
{
sequence = 0;
}
else if( payload->m_part == 1 )
{
sequence = (++m_sequenceCounter) & 0x0f;
sequence |= 0x10; // Sequenced, first frame
}
if( payload->m_part == 2 )
{
sequence = m_sequenceCounter & 0x0f;
sequence |= 0x30; // Sequenced, second frame
}
/* at most, the payload will be 28 bytes + 1 byte for the Sequence. */
uint8 plaintextmsg[32];
plaintextmsg[0] = sequence;
for (int i = 0; i < payload->m_length; i++)
plaintextmsg[i+1] = payload->m_data[i];
/* Append the message payload after encrypting it with AES-OFB (key is EncryptPassword,
* full IV (16 bytes - 8 Random and 8 NONCE) and payload.m_data
*/
PrintHex("Input Packet:", plaintextmsg, payload->m_length+1);
#ifdef DEBUG
PrintHex("IV:", initializationVector, 16);
#endif
uint8 encryptedpayload[30];
aes_mode_reset(this->EncryptKey);
if (aes_ofb_encrypt(plaintextmsg, encryptedpayload, payload->m_length+1, initializationVector, this->EncryptKey) == EXIT_FAILURE) {
Log::Write(LogLevel_Warning, GetNodeId(), "Failed to Encrypt Packet");
delete msg;
return false;
}
#ifdef DEBUG
PrintHex("Encrypted Output", encryptedpayload, payload->m_length+1);
/* The Following Code attempts to Decrypt the Packet and Verify */
/* the first 8 bytes of a outgoing IV are random */
for (int i = 0; i < 8; i++) {
//initializationVector[i] = (rand()%0xFF)+1;
initializationVector[i] = 0xAA;
}
/* the remaining 8 bytes are the NONCE we got from the device */
for (int i = 0; i < 8; i++) {
initializationVector[8+i] = _nonce[i];
}
aes_mode_reset(this->EncryptKey);
uint8 tmpoutput[16];
if (aes_ofb_encrypt(encryptedpayload, tmpoutput, payload->m_length+1, initializationVector, this->EncryptKey) == EXIT_FAILURE) {
Log::Write(LogLevel_Warning, GetNodeId(), "Failed to Encrypt Packet");
delete msg;
return false;
}
PrintHex("Decrypted output", tmpoutput, payload->m_length+1);
#endif
for(int i=0; i<payload->m_length+1; ++i )
{
msg->Append( encryptedpayload[i] );
}
// Append the nonce identifier :)
msg->Append(_nonce[0]);
/* Append space for the authentication data Set with AES-CBCMAC (key is AuthPassword,
* Full IV (16 bytes - 8 random and 8 NONCE) and sequence|SrcNode|DstNode|payload.m_length|payload.m_data
*
*/
/* Regenerate IV */
/* the first 8 bytes of a outgoing IV are random */
for (int i = 0; i < 8; i++) {
//initializationVector[i] = (rand()%0xFF)+1;
initializationVector[i] = 0xAA;
}
/* the remaining 8 bytes are the NONCE we got from the device */
for (int i = 0; i < 8; i++) {
initializationVector[8+i] = _nonce[i];
}
uint8 mac[8];
this->GenerateAuthentication(&msg->GetBuffer()[7], msg->GetLength()+2, GetDriver()->GetNodeId(), GetNodeId(), initializationVector, mac);
for(int i=0; i<8; ++i )
{
msg->Append( mac[i] );
}
#ifdef DEBUG
PrintHex("Auth", mac, 8);
#endif
msg->Append( GetDriver()->GetTransmitOptions() );
#ifdef DEBUG
PrintHex("Outgoing", msg->GetBuffer(), msg->GetLength());
#endif
GetDriver()->SendMsg(msg, Driver::MsgQueue_Security);
/* finally, if the message we just sent is a NetworkKeySet, then we need to reset our Network Key here
* as the reply we will get back will be encrypted with the new Network key
*/
if ((this->m_networkkeyset == false) && (payload->m_data[0] == 0x98) && (payload->m_data[1] == 0x06)) {
Log::Write(LogLevel_Info, GetNodeId(), "Reseting Network Key after Inclusion");
this->m_networkkeyset = true;
SetupNetworkKey();
}
delete payload;
return true;
}
