Skip to content

Commit 915e914

Browse files
eltoseltos
committed
Move LTK signaling and confirm to separate method
1 parent 750b7f1 commit 915e914

File tree

4 files changed

+80
-167
lines changed

4 files changed

+80
-167
lines changed

‎src/utility/HCI.cpp‎

Lines changed: 2 additions & 87 deletions
Original file line numberDiff line numberDiff line change
@@ -1337,106 +1337,21 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[])
13371337
break;
13381338
}
13391339

1340-
uint8_t encryption = ATT.getPeerEncryption(connectionHandle);
13411340

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

13441343
#ifdef _BLE_TRACE_
13451344
Serial.println("Stored our DHKey:");
13461345
btct.printBytes(DHKey,32);
13471346
#endif
1348-
encryption |= PEER_ENCRYPTION::DH_KEY_CALULATED;
1347+
uint8_t encryption = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::DH_KEY_CALULATED;
13491348
ATT.setPeerEncryption(connectionHandle, encryption);
13501349

13511350
if((encryption & PEER_ENCRYPTION::RECEIVED_DH_CHECK) > 0){
13521351
#ifdef _BLE_TRACE_
13531352
Serial.println("Recieved DHKey check already so calculate f5, f6 now.");
13541353
#endif
1355-
1356-
uint8_t BD_ADDR_REMOTE[7];
1357-
ATT.getPeerAddrWithType(connectionHandle, BD_ADDR_REMOTE);
1358-
1359-
1360-
uint8_t MacKey[16];
1361-
uint8_t localAddress[7];
1362-
1363-
memcpy(&localAddress[1],HCI.localAddr,6);
1364-
localAddress[0] = 0; // IOT 33 uses a static address
1365-
1366-
btct.f5(HCI.DHKey,HCI.Na,HCI.Nb,BD_ADDR_REMOTE,localAddress,MacKey,HCI.LTK);
1367-
1368-
uint8_t Ea[16];
1369-
uint8_t Eb[16];
1370-
uint8_t R[16];
1371-
uint8_t MasterIOCap[3];
1372-
uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP};
1373-
1374-
ATT.getPeerIOCap(connectionHandle, MasterIOCap);
1375-
for(int i=0; i<16; i++) R[i] = 0;
1376-
1377-
btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, BD_ADDR_REMOTE, localAddress, Ea);
1378-
btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, BD_ADDR_REMOTE, Eb);
1379-
1380-
1381-
#ifdef _BLE_TRACE_
1382-
Serial.println("Calculate f5, f6:");
1383-
Serial.print("DH : ");
1384-
btct.printBytes(HCI.DHKey,32);
1385-
Serial.print("Na : ");
1386-
btct.printBytes(HCI.Na,16);
1387-
Serial.print("Nb : ");
1388-
btct.printBytes(HCI.Nb,16);
1389-
Serial.print("MAC : ");
1390-
btct.printBytes(MacKey,16);
1391-
// Serial.print("Expected MAC: ");
1392-
// printBytes(EXPECTED_MAC, 16);
1393-
Serial.print("LTK : ");
1394-
btct.printBytes(HCI.LTK,16);
1395-
// Serial.print("Expected LTK: ");
1396-
// printBytes(EXPECTED_LTK, 16);
1397-
Serial.print("Expected Ex : ");
1398-
btct.printBytes(HCI.remoteDHKeyCheckBuffer, 16);
1399-
Serial.print("Ea : ");
1400-
btct.printBytes(Ea, 16);
1401-
Serial.print("Eb : ");
1402-
btct.printBytes(Eb,16);
1403-
Serial.print("Local Addr : ");
1404-
btct.printBytes(localAddress, 7);
1405-
Serial.print("LocalIOCap : ");
1406-
btct.printBytes(SlaveIOCap, 3);
1407-
Serial.print("MasterAddr : ");
1408-
btct.printBytes(BD_ADDR_REMOTE, 7);
1409-
Serial.print("MasterIOCAP : ");
1410-
btct.printBytes(MasterIOCap, 3);
1411-
Serial.println("Send Eb Back.");
1412-
#endif
1413-
// Check if RemoteDHKeyCheck = Ea
1414-
bool EaCheck = true;
1415-
for(int i = 0; i < 16; i++){
1416-
if (Ea[i] != HCI.remoteDHKeyCheckBuffer[i]){
1417-
EaCheck = false;
1418-
}
1419-
}
1420-
1421-
if (EaCheck){
1422-
// Send our confirmation value to complete authentication stage 2
1423-
uint8_t ret[17];
1424-
ret[0] = CONNECTION_PAIRING_DHKEY_CHECK;
1425-
for(int i=0; i<sizeof(Eb); i++){
1426-
ret[sizeof(Eb)-i] = Eb[i];
1427-
}
1428-
HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(ret), ret );
1429-
ATT.setPeerEncryption(connectionHandle, encryption | PEER_ENCRYPTION::SENT_DH_CHECK);
1430-
1431-
} else {
1432-
// If check fails, abort
1433-
#ifdef _BLE_TRACE_
1434-
Serial.println("Error: DHKey check failed - Aborting");
1435-
#endif
1436-
uint8_t ret[2] = {CONNECTION_PAIRING_FAILED, 0x0B}; // DHKey Check Faile
1437-
HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(ret), ret);
1438-
ATT.setPeerEncryption(connectionHandle, NO_ENCRYPTION);
1439-
}
1354+
L2CAPSignaling.smCalculateLTKandConfirm(connectionHandle, HCI.remoteDHKeyCheckBuffer);
14401355

14411356
}else{
14421357
#ifdef _BLE_TRACE_

‎src/utility/HCI.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,8 @@
2222

2323
#include <Arduino.h>
2424

25+
#include "L2CAPSignaling.h"
26+
2527
#define OGF_LINK_CTL 0x01
2628
#define OGF_HOST_CTL 0x03
2729
#define OGF_INFO_PARAM 0x04

‎src/utility/L2CAPSignaling.cpp‎

Lines changed: 70 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -278,19 +278,18 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t
278278
else if(code == CONNECTION_PAIRING_DHKEY_CHECK)
279279
{
280280
uint8_t RemoteDHKeyCheck[16];
281-
uint8_t BD_ADDR_REMOTE[7];
282-
ATT.getPeerAddrWithType(connectionHandle, BD_ADDR_REMOTE);
283281
for(int i=0; i<16; i++) RemoteDHKeyCheck[15-i] = l2capSignalingHdr->data[i];
284-
uint8_t encryptionState = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::RECEIVED_DH_CHECK;
285-
282+
286283

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

293-
HCI.readBdAddr();
290+
291+
292+
uint8_t encryptionState = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::RECEIVED_DH_CHECK;
294293
ATT.setPeerEncryption(connectionHandle, encryptionState);
295294
if((encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED) == 0){
296295
#ifdef _BLE_TRACE_
@@ -301,89 +300,80 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t
301300

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

305-
uint8_t MacKey[16];
306-
uint8_t localAddress[7];
307-
308-
memcpy(&localAddress[1],HCI.localAddr,6);
309-
localAddress[0] = 0; // IOT 33 uses a static address
305+
}
306+
}
307+
}
310308

311-
btct.f5(HCI.DHKey,HCI.Na,HCI.Nb,BD_ADDR_REMOTE,localAddress,MacKey,HCI.LTK);
312-
313-
uint8_t Ea[16];
314-
uint8_t Eb[16];
315-
uint8_t R[16];
316-
uint8_t MasterIOCap[3];
317-
uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP};
318-
319-
ATT.getPeerIOCap(connectionHandle, MasterIOCap);
320-
for(int i=0; i<16; i++) R[i] = 0;
321-
322-
btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, BD_ADDR_REMOTE, localAddress, Ea);
323-
btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, BD_ADDR_REMOTE, Eb);
324-
309+
void L2CAPSignalingClass::smCalculateLTKandConfirm(uint16_t handle, uint8_t expectedEa[])
310+
{ // Authentication stage 2: LTK Calculation
311+
312+
uint8_t localAddress[7];
313+
uint8_t remoteAddress[7];
314+
ATT.getPeerAddrWithType(handle, remoteAddress);
315+
316+
HCI.readBdAddr();
317+
memcpy(&localAddress[1],HCI.localAddr,6);
318+
localAddress[0] = 0; // IOT 33 uses a static address // TODO: confirm for Nano BLE
319+
320+
// Compute the LTK and MacKey
321+
uint8_t MacKey[16];
322+
btct.f5(HCI.DHKey, HCI.Na, HCI.Nb, remoteAddress, localAddress, MacKey, HCI.LTK);
323+
324+
// Compute Ea and Eb
325+
uint8_t Ea[16];
326+
uint8_t Eb[16];
327+
uint8_t R[16];
328+
uint8_t MasterIOCap[3];
329+
uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP};
330+
331+
ATT.getPeerIOCap(handle, MasterIOCap);
332+
for(int i=0; i<16; i++) R[i] = 0;
333+
334+
btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, remoteAddress, localAddress, Ea);
335+
btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, remoteAddress, Eb);
325336

326337
#ifdef _BLE_TRACE_
327-
Serial.println("Calculate f5, f6:");
328-
Serial.print("DH : ");
329-
btct.printBytes(HCI.DHKey,32);
330-
Serial.print("Na : ");
331-
btct.printBytes(HCI.Na,16);
332-
Serial.print("Nb : ");
333-
btct.printBytes(HCI.Nb,16);
334-
Serial.print("MAC : ");
335-
btct.printBytes(MacKey,16);
336-
// Serial.print("Expected MAC: ");
337-
// printBytes(EXPECTED_MAC, 16);
338-
Serial.print("LTK : ");
339-
btct.printBytes(HCI.LTK,16);
340-
// Serial.print("Expected LTK: ");
341-
// printBytes(EXPECTED_LTK, 16);
342-
Serial.print("Expected Ex : ");
343-
btct.printBytes(RemoteDHKeyCheck, 16);
344-
Serial.print("Ea : ");
345-
btct.printBytes(Ea, 16);
346-
Serial.print("Eb : ");
347-
btct.printBytes(Eb,16);
348-
Serial.print("Local Addr : ");
349-
btct.printBytes(localAddress, 7);
350-
Serial.print("LocalIOCap : ");
351-
btct.printBytes(SlaveIOCap, 3);
352-
Serial.print("MasterAddr : ");
353-
btct.printBytes(BD_ADDR_REMOTE, 7);
354-
Serial.print("MasterIOCAP : ");
355-
btct.printBytes(MasterIOCap, 3);
356-
Serial.println("Send Eb Back.");
338+
Serial.println("Calculate and confirm LTK via f5, f6:");
339+
Serial.print("DHKey : "); btct.printBytes(HCI.DHKey,32);
340+
Serial.print("Na : "); btct.printBytes(HCI.Na,16);
341+
Serial.print("Nb : "); btct.printBytes(HCI.Nb,16);
342+
Serial.print("MacKey : "); btct.printBytes(MacKey,16);
343+
Serial.print("LTK : "); btct.printBytes(HCI.LTK,16);
344+
Serial.print("Expected Ea: "); btct.printBytes(expectedEa, 16);
345+
Serial.print("Ea : "); btct.printBytes(Ea, 16);
346+
Serial.print("Eb : "); btct.printBytes(Eb,16);
347+
Serial.print("Local Addr : "); btct.printBytes(localAddress, 7);
348+
Serial.print("LocalIOCap : "); btct.printBytes(SlaveIOCap, 3);
349+
Serial.print("MasterAddr : "); btct.printBytes(remoteAddress, 7);
350+
Serial.print("MasterIOCAP: "); btct.printBytes(MasterIOCap, 3);
357351
#endif
358352

359-
// Check if RemoteDHKeyCheck = Ea
360-
bool EaCheck = true;
361-
for(int i = 0; i < 16; i++){
362-
if (Ea[i] != RemoteDHKeyCheck[i]){
363-
EaCheck = false;
364-
}
365-
}
366-
367-
if (EaCheck){
368-
// Send our confirmation value to complete authentication stage 2
369-
uint8_t ret[17];
370-
ret[0] = CONNECTION_PAIRING_DHKEY_CHECK;
371-
for(int i=0; i<sizeof(Eb); i++){
372-
ret[sizeof(Eb)-i] = Eb[i];
373-
}
374-
HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(ret), ret );
375-
ATT.setPeerEncryption(connectionHandle, encryptionState | PEER_ENCRYPTION::SENT_DH_CHECK);
376-
377-
} else {
378-
// If check fails, abort
353+
// Check if Ea = expectedEa
354+
if (memcmp(Ea, expectedEa, 16) == 0){
355+
// Check ok
356+
// Send our confirmation value to complete authentication stage 2
357+
uint8_t ret[17];
358+
ret[0] = CONNECTION_PAIRING_DHKEY_CHECK;
359+
for(int i=0; i<sizeof(Eb); i++){
360+
ret[sizeof(Eb)-i] = Eb[i];
361+
}
362+
HCI.sendAclPkt(handle, SECURITY_CID, sizeof(ret), ret );
363+
uint8_t encryption = ATT.getPeerEncryption(handle) | PEER_ENCRYPTION::SENT_DH_CHECK;
364+
ATT.setPeerEncryption(handle, encryption);
379365
#ifdef _BLE_TRACE_
380-
Serial.println("Error: DHKey check failed - Aborting");
366+
Serial.println("DHKey check ok - send Eb back");
367+
#endif
368+
369+
} else {
370+
// Check failed, abort pairing
371+
uint8_t ret[2] = {CONNECTION_PAIRING_FAILED, 0x0B}; // 0x0B = DHKey Check Failed
372+
HCI.sendAclPkt(handle, SECURITY_CID, sizeof(ret), ret);
373+
ATT.setPeerEncryption(handle, NO_ENCRYPTION);
374+
#ifdef _BLE_TRACE_
375+
Serial.println("Error: DHKey check failed - Aborting");
381376
#endif
382-
uint8_t ret[2] = {CONNECTION_PAIRING_FAILED, 0x0B}; // DHKey Check Faile
383-
HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(ret), ret);
384-
ATT.setPeerEncryption(connectionHandle, NO_ENCRYPTION);
385-
}
386-
}
387377
}
388378
}
389379

‎src/utility/L2CAPSignaling.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,10 +64,16 @@ class L2CAPSignalingClass {
6464

6565
virtual void setSupervisionTimeout(uint16_t supervisionTimeout);
6666

67+
68+
69+
virtual void smCalculateLTKandConfirm(uint16_t handle, uint8_t expectedEa[]);
70+
71+
6772
private:
6873
virtual void connectionParameterUpdateRequest(uint16_t handle, uint8_t identifier, uint8_t dlen, uint8_t data[]);
6974
virtual void connectionParameterUpdateResponse(uint16_t handle, uint8_t identifier, uint8_t dlen, uint8_t data[]);
7075

76+
7177
private:
7278
uint16_t _minInterval;
7379
uint16_t _maxInterval;

0 commit comments

Comments
 (0)