Skip to content

Commit

Permalink
Move LTK signaling and confirm to separate method
Browse files Browse the repository at this point in the history
  • Loading branch information
@eltos
eltos committed Jan 4, 2021
1 parent 750b7f1 commit 915e914
Show file tree
Hide file tree
Showing 4 changed files with 80 additions and 167 deletions.
89 changes: 2 additions & 87 deletions src/utility/HCI.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1337,106 +1337,21 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[])
break;
}

uint8_t encryption = ATT.getPeerEncryption(connectionHandle);

for(int i=0; i<32; i++) DHKey[31-i] = evtLeDHKeyComplete->DHKey[i];

#ifdef _BLE_TRACE_
Serial.println("Stored our DHKey:");
btct.printBytes(DHKey,32);
#endif
encryption |= PEER_ENCRYPTION::DH_KEY_CALULATED;
uint8_t encryption = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::DH_KEY_CALULATED;
ATT.setPeerEncryption(connectionHandle, encryption);

if((encryption & PEER_ENCRYPTION::RECEIVED_DH_CHECK) > 0){
#ifdef _BLE_TRACE_
Serial.println("Recieved DHKey check already so calculate f5, f6 now.");
#endif

uint8_t BD_ADDR_REMOTE[7];
ATT.getPeerAddrWithType(connectionHandle, BD_ADDR_REMOTE);


uint8_t MacKey[16];
uint8_t localAddress[7];

memcpy(&localAddress[1],HCI.localAddr,6);
localAddress[0] = 0; // IOT 33 uses a static address

btct.f5(HCI.DHKey,HCI.Na,HCI.Nb,BD_ADDR_REMOTE,localAddress,MacKey,HCI.LTK);

uint8_t Ea[16];
uint8_t Eb[16];
uint8_t R[16];
uint8_t MasterIOCap[3];
uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP};

ATT.getPeerIOCap(connectionHandle, MasterIOCap);
for(int i=0; i<16; i++) R[i] = 0;

btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, BD_ADDR_REMOTE, localAddress, Ea);
btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, BD_ADDR_REMOTE, Eb);


#ifdef _BLE_TRACE_
Serial.println("Calculate f5, f6:");
Serial.print("DH : ");
btct.printBytes(HCI.DHKey,32);
Serial.print("Na : ");
btct.printBytes(HCI.Na,16);
Serial.print("Nb : ");
btct.printBytes(HCI.Nb,16);
Serial.print("MAC : ");
btct.printBytes(MacKey,16);
// Serial.print("Expected MAC: ");
// printBytes(EXPECTED_MAC, 16);
Serial.print("LTK : ");
btct.printBytes(HCI.LTK,16);
// Serial.print("Expected LTK: ");
// printBytes(EXPECTED_LTK, 16);
Serial.print("Expected Ex : ");
btct.printBytes(HCI.remoteDHKeyCheckBuffer, 16);
Serial.print("Ea : ");
btct.printBytes(Ea, 16);
Serial.print("Eb : ");
btct.printBytes(Eb,16);
Serial.print("Local Addr : ");
btct.printBytes(localAddress, 7);
Serial.print("LocalIOCap : ");
btct.printBytes(SlaveIOCap, 3);
Serial.print("MasterAddr : ");
btct.printBytes(BD_ADDR_REMOTE, 7);
Serial.print("MasterIOCAP : ");
btct.printBytes(MasterIOCap, 3);
Serial.println("Send Eb Back.");
#endif
// Check if RemoteDHKeyCheck = Ea
bool EaCheck = true;
for(int i = 0; i < 16; i++){
if (Ea[i] != HCI.remoteDHKeyCheckBuffer[i]){
EaCheck = false;
}
}

if (EaCheck){
// Send our confirmation value to complete authentication stage 2
uint8_t ret[17];
ret[0] = CONNECTION_PAIRING_DHKEY_CHECK;
for(int i=0; i<sizeof(Eb); i++){
ret[sizeof(Eb)-i] = Eb[i];
}
HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(ret), ret );
ATT.setPeerEncryption(connectionHandle, encryption | PEER_ENCRYPTION::SENT_DH_CHECK);

} else {
// If check fails, abort
#ifdef _BLE_TRACE_
Serial.println("Error: DHKey check failed - Aborting");
#endif
uint8_t ret[2] = {CONNECTION_PAIRING_FAILED, 0x0B}; // DHKey Check Faile
HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(ret), ret);
ATT.setPeerEncryption(connectionHandle, NO_ENCRYPTION);
}
L2CAPSignaling.smCalculateLTKandConfirm(connectionHandle, HCI.remoteDHKeyCheckBuffer);

}else{
#ifdef _BLE_TRACE_
Expand Down
2 changes: 2 additions & 0 deletions src/utility/HCI.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@

#include <Arduino.h>

#include "L2CAPSignaling.h"

#define OGF_LINK_CTL 0x01
#define OGF_HOST_CTL 0x03
#define OGF_INFO_PARAM 0x04
Expand Down
150 changes: 70 additions & 80 deletions src/utility/L2CAPSignaling.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -278,19 +278,18 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t
else if(code == CONNECTION_PAIRING_DHKEY_CHECK)
{
uint8_t RemoteDHKeyCheck[16];
uint8_t BD_ADDR_REMOTE[7];
ATT.getPeerAddrWithType(connectionHandle, BD_ADDR_REMOTE);
for(int i=0; i<16; i++) RemoteDHKeyCheck[15-i] = l2capSignalingHdr->data[i];
uint8_t encryptionState = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::RECEIVED_DH_CHECK;



#ifdef _BLE_TRACE_
Serial.println("[Info] DH Key check");
Serial.print("Remote DHKey Check: ");
btct.printBytes(RemoteDHKeyCheck, 16);
#endif

HCI.readBdAddr();


uint8_t encryptionState = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::RECEIVED_DH_CHECK;
ATT.setPeerEncryption(connectionHandle, encryptionState);
if((encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED) == 0){
#ifdef _BLE_TRACE_
Expand All @@ -301,89 +300,80 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t

} else {
// We've already calculated the DHKey so we can calculate our check and send it.
smCalculateLTKandConfirm(connectionHandle, RemoteDHKeyCheck);

uint8_t MacKey[16];
uint8_t localAddress[7];

memcpy(&localAddress[1],HCI.localAddr,6);
localAddress[0] = 0; // IOT 33 uses a static address
}
}
}

btct.f5(HCI.DHKey,HCI.Na,HCI.Nb,BD_ADDR_REMOTE,localAddress,MacKey,HCI.LTK);

uint8_t Ea[16];
uint8_t Eb[16];
uint8_t R[16];
uint8_t MasterIOCap[3];
uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP};

ATT.getPeerIOCap(connectionHandle, MasterIOCap);
for(int i=0; i<16; i++) R[i] = 0;

btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, BD_ADDR_REMOTE, localAddress, Ea);
btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, BD_ADDR_REMOTE, Eb);

void L2CAPSignalingClass::smCalculateLTKandConfirm(uint16_t handle, uint8_t expectedEa[])
{ // Authentication stage 2: LTK Calculation

uint8_t localAddress[7];
uint8_t remoteAddress[7];
ATT.getPeerAddrWithType(handle, remoteAddress);

HCI.readBdAddr();
memcpy(&localAddress[1],HCI.localAddr,6);
localAddress[0] = 0; // IOT 33 uses a static address // TODO: confirm for Nano BLE

// Compute the LTK and MacKey
uint8_t MacKey[16];
btct.f5(HCI.DHKey, HCI.Na, HCI.Nb, remoteAddress, localAddress, MacKey, HCI.LTK);

// Compute Ea and Eb
uint8_t Ea[16];
uint8_t Eb[16];
uint8_t R[16];
uint8_t MasterIOCap[3];
uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP};

ATT.getPeerIOCap(handle, MasterIOCap);
for(int i=0; i<16; i++) R[i] = 0;

btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, remoteAddress, localAddress, Ea);
btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, remoteAddress, Eb);

#ifdef _BLE_TRACE_
Serial.println("Calculate f5, f6:");
Serial.print("DH : ");
btct.printBytes(HCI.DHKey,32);
Serial.print("Na : ");
btct.printBytes(HCI.Na,16);
Serial.print("Nb : ");
btct.printBytes(HCI.Nb,16);
Serial.print("MAC : ");
btct.printBytes(MacKey,16);
// Serial.print("Expected MAC: ");
// printBytes(EXPECTED_MAC, 16);
Serial.print("LTK : ");
btct.printBytes(HCI.LTK,16);
// Serial.print("Expected LTK: ");
// printBytes(EXPECTED_LTK, 16);
Serial.print("Expected Ex : ");
btct.printBytes(RemoteDHKeyCheck, 16);
Serial.print("Ea : ");
btct.printBytes(Ea, 16);
Serial.print("Eb : ");
btct.printBytes(Eb,16);
Serial.print("Local Addr : ");
btct.printBytes(localAddress, 7);
Serial.print("LocalIOCap : ");
btct.printBytes(SlaveIOCap, 3);
Serial.print("MasterAddr : ");
btct.printBytes(BD_ADDR_REMOTE, 7);
Serial.print("MasterIOCAP : ");
btct.printBytes(MasterIOCap, 3);
Serial.println("Send Eb Back.");
Serial.println("Calculate and confirm LTK via f5, f6:");
Serial.print("DHKey : "); btct.printBytes(HCI.DHKey,32);
Serial.print("Na : "); btct.printBytes(HCI.Na,16);
Serial.print("Nb : "); btct.printBytes(HCI.Nb,16);
Serial.print("MacKey : "); btct.printBytes(MacKey,16);
Serial.print("LTK : "); btct.printBytes(HCI.LTK,16);
Serial.print("Expected Ea: "); btct.printBytes(expectedEa, 16);
Serial.print("Ea : "); btct.printBytes(Ea, 16);
Serial.print("Eb : "); btct.printBytes(Eb,16);
Serial.print("Local Addr : "); btct.printBytes(localAddress, 7);
Serial.print("LocalIOCap : "); btct.printBytes(SlaveIOCap, 3);
Serial.print("MasterAddr : "); btct.printBytes(remoteAddress, 7);
Serial.print("MasterIOCAP: "); btct.printBytes(MasterIOCap, 3);
#endif

// Check if RemoteDHKeyCheck = Ea
bool EaCheck = true;
for(int i = 0; i < 16; i++){
if (Ea[i] != RemoteDHKeyCheck[i]){
EaCheck = false;
}
}

if (EaCheck){
// Send our confirmation value to complete authentication stage 2
uint8_t ret[17];
ret[0] = CONNECTION_PAIRING_DHKEY_CHECK;
for(int i=0; i<sizeof(Eb); i++){
ret[sizeof(Eb)-i] = Eb[i];
}
HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(ret), ret );
ATT.setPeerEncryption(connectionHandle, encryptionState | PEER_ENCRYPTION::SENT_DH_CHECK);

} else {
// If check fails, abort
// Check if Ea = expectedEa
if (memcmp(Ea, expectedEa, 16) == 0){
// Check ok
// Send our confirmation value to complete authentication stage 2
uint8_t ret[17];
ret[0] = CONNECTION_PAIRING_DHKEY_CHECK;
for(int i=0; i<sizeof(Eb); i++){
ret[sizeof(Eb)-i] = Eb[i];
}
HCI.sendAclPkt(handle, SECURITY_CID, sizeof(ret), ret );
uint8_t encryption = ATT.getPeerEncryption(handle) | PEER_ENCRYPTION::SENT_DH_CHECK;
ATT.setPeerEncryption(handle, encryption);
#ifdef _BLE_TRACE_
Serial.println("Error: DHKey check failed - Aborting");
Serial.println("DHKey check ok - send Eb back");
#endif

} else {
// Check failed, abort pairing
uint8_t ret[2] = {CONNECTION_PAIRING_FAILED, 0x0B}; // 0x0B = DHKey Check Failed
HCI.sendAclPkt(handle, SECURITY_CID, sizeof(ret), ret);
ATT.setPeerEncryption(handle, NO_ENCRYPTION);
#ifdef _BLE_TRACE_
Serial.println("Error: DHKey check failed - Aborting");
#endif
uint8_t ret[2] = {CONNECTION_PAIRING_FAILED, 0x0B}; // DHKey Check Faile
HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(ret), ret);
ATT.setPeerEncryption(connectionHandle, NO_ENCRYPTION);
}
}
}
}

Expand Down
6 changes: 6 additions & 0 deletions src/utility/L2CAPSignaling.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,10 +64,16 @@ class L2CAPSignalingClass {

virtual void setSupervisionTimeout(uint16_t supervisionTimeout);



virtual void smCalculateLTKandConfirm(uint16_t handle, uint8_t expectedEa[]);


private:
virtual void connectionParameterUpdateRequest(uint16_t handle, uint8_t identifier, uint8_t dlen, uint8_t data[]);
virtual void connectionParameterUpdateResponse(uint16_t handle, uint8_t identifier, uint8_t dlen, uint8_t data[]);


private:
uint16_t _minInterval;
uint16_t _maxInterval;
Expand Down

0 comments on commit 915e914

Please sign in to comment.