Handle remote DHKey confirmation before own DHKey

arduino-libraries · Jan 3, 2021 · 0aa2e87 · 0aa2e87
1 parent 7ecf299
commit 0aa2e87
Show file tree

Hide file tree

Showing 3 changed files with 83 additions and 4 deletions.
diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp
@@ -1345,13 +1345,83 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[])
 #endif
           encryption |= 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_
-          if(encryption | PEER_ENCRYPTION::RECEIVED_DH_CHECK){
-            Serial.println("Recieved DHKey check already so calculate f5, f6.");
+            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
+            uint8_t ret[17];
+            ret[0] = 0x0d;
+            for(int i=0; i<sizeof(Eb); i++){
+              ret[sizeof(Eb)-i] = Eb[i];
+            }
+            HCI.sendAclPkt(connectionHandle, 0x06, sizeof(ret), ret );
+            ATT.setPeerEncryption(connectionHandle, encryption | PEER_ENCRYPTION::SENT_DH_CHECK);
           }else{
+#ifdef _BLE_TRACE_
             Serial.println("Waiting on other DHKey check before calculating.");
+#endif
           }
         }else{
+#ifdef _BLE_TRACE_
           Serial.print("Key generation error: 0x");
           Serial.println(evtLeDHKeyComplete->status, HEX);
 #endif

diff --git a/src/utility/HCI.h b/src/utility/HCI.h
@@ -111,6 +111,7 @@ class HCIClass {
   // TODO: Send command be private again & use ATT implementation within ATT.
   virtual int sendCommand(uint16_t opcode, uint8_t plen = 0, void* parameters = NULL);
   uint8_t remotePublicKeyBuffer[64];
+  uint8_t remoteDHKeyCheckBuffer[16];
   uint8_t Na[16];
   uint8_t Nb[16];
   uint8_t DHKey[32];

diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp
@@ -290,7 +290,14 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t
 
     HCI.readBdAddr();
     ATT.setPeerEncryption(connectionHandle, encryptionState);
-    if((encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED) > 0){
+    if((encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED) == 0){
+#ifdef _BLE_TRACE_
+      Serial.println("DHKey not yet ready, will calculate f5, f6 later");
+#endif
+      // store RemoteDHKeyCheck for later check
+      memcpy(HCI.remoteDHKeyCheckBuffer,RemoteDHKeyCheck,16);
+
+    } else {
       // We've already calculated the DHKey so we can calculate our check and send it.
 
       uint8_t MacKey[16];
@@ -312,7 +319,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t
 
       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:");
@@ -353,6 +360,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t
         ret[sizeof(Eb)-i] = Eb[i];
       }
       HCI.sendAclPkt(connectionHandle, 0x06, sizeof(ret), ret );
+      ATT.setPeerEncryption(connectionHandle, encryptionState | PEER_ENCRYPTION::SENT_DH_CHECK);
     }
   }
 }