diff --git a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino index 338a374e..d4953b03 100644 --- a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino +++ b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino @@ -44,34 +44,34 @@ void setup() { // IRKs are keys that identify the true owner of a random mac address. // Add IRKs of devices you are bonded with. - BLE.setGetIRKs([](uint8_t* nIRKs, uint8_t** BADDR_TYPES, uint8_t*** BDAddrs, uint8_t*** IRKs){ + BLE.setGetIRKs([](uint8_t* nIRKs, uint8_t** BDaddrTypes, uint8_t*** BDAddrs, uint8_t*** IRKs){ // Set to number of devices *nIRKs = 2; *BDAddrs = new uint8_t*[*nIRKs]; *IRKs = new uint8_t*[*nIRKs]; - *BADDR_TYPES = new uint8_t[*nIRKs]; + *BDaddrTypes = new uint8_t[*nIRKs]; // Set these to the mac and IRK for your bonded devices as printed in the serial console after bonding. - uint8_t iPhoneMac [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPhoneIRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device1Mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device1IRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPadMac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPadIRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, }; + uint8_t device2Mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device2IRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - (*BADDR_TYPES)[0] = 0; - (*IRKs)[0] = new uint8_t[16]; - memcpy((*IRKs)[0],iPhoneIRK,16); + (*BDaddrTypes)[0] = 0; // Type 0 is for pubc address, type 1 is for static random (*BDAddrs)[0] = new uint8_t[6]; - memcpy((*BDAddrs)[0], iPhoneMac, 6); + (*IRKs)[0] = new uint8_t[16]; + memcpy((*IRKs)[0] , device1IRK,16); + memcpy((*BDAddrs)[0], device1Mac, 6); - (*BADDR_TYPES)[1] = 0; - (*IRKs)[1] = new uint8_t[16]; - memcpy((*IRKs)[1],iPadIRK,16); + (*BDaddrTypes)[1] = 0; (*BDAddrs)[1] = new uint8_t[6]; - memcpy((*BDAddrs)[1], iPadMac, 6); + (*IRKs)[1] = new uint8_t[16]; + memcpy((*IRKs)[1] , device2IRK,16); + memcpy((*BDAddrs)[1], device2Mac, 6); return 1; @@ -83,17 +83,18 @@ void setup() { btct.printBytes(address,6); // Set these to the MAC and LTK of your devices after bonding. - uint8_t iPhoneMac [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPhoneLTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPadMac [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPadLTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device1Mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device1LTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device2Mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device2LTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - if(memcmp(iPhoneMac, address, 6)==0){ - memcpy(LTK, iPhoneLTK, 16); + if(memcmp(device1Mac, address, 6) == 0) { + memcpy(LTK, device1LTK, 16); + return 1; + }else if(memcmp(device2Mac, address, 6) == 0) { + memcpy(LTK, device2LTK, 16); return 1; - }else if(memcmp(iPadMac, address, 6)==0){ - memcpy(LTK, iPadLTK, 16); } return 0; }); @@ -112,7 +113,8 @@ void setup() { return 1; }); - while(1){// begin initialization + while(1){ + // begin initialization if (!BLE.begin()) { Serial.println("starting BLE failed!"); delay(200); @@ -135,7 +137,7 @@ void setup() { batteryService.addCharacteristic(stringcharacteristic); batteryService.addCharacteristic(secretValue); - BLE.addService(batteryService); // Add the battery service + BLE.addService(batteryService); // Add the battery service batteryLevelChar.writeValue(oldBatteryLevel); // set initial value for this characteristic char* stringCharValue = new char[32]; stringCharValue = "string"; diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index 6ef6bc31..18556413 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -260,7 +260,10 @@ void ATTClass::addConnection(uint16_t handle, uint8_t role, uint8_t peerBdaddrTy Serial.println("Found match."); #endif }else{ - memset(_peers[peerIndex].resolvedAddress, 0, 6); +#ifdef _BLE_TRACE_ + Serial.println("No matching MAC"); +#endif + memset(&_peers[peerIndex].resolvedAddress, 0, 6); } if (_eventHandlers[BLEConnected]) { diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 376cfb9c..4dc78460 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -1218,10 +1218,11 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) uint16_t minLength; uint16_t maxLength; } remoteConnParamReqReply; - memcpy(&remoteConnParamReqReply, &remoteConnParamReq->connectionHandle, sizeof(remoteConnParamReq-1)); + memcpy(&remoteConnParamReqReply, &remoteConnParamReq->connectionHandle, sizeof(RemoteConnParamReq)-1); + remoteConnParamReqReply.minLength = 0x000F; remoteConnParamReqReply.maxLength = 0x0FFF; - sendCommand(OGF_LE_CTL << 10 | 0x20, sizeof(remoteConnParamReqReply), &remoteConnParamReqReply); + sendCommand(OGF_LE_CTL << 10 | 0x20, sizeof(RemoteConnParamReqReply), &remoteConnParamReqReply); break; } case READ_LOCAL_P256_COMPLETE:{ @@ -1381,7 +1382,7 @@ int HCIClass::leEncrypt(uint8_t* key, uint8_t* plaintext, uint8_t* status, uint8 leEncryptCommand.plaintext[15-i] = plaintext[i]; } - int res = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::ENCRYPT, sizeof(leEncryptCommand), &leEncryptCommand); + int res = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::ENCRYPT, 32, &leEncryptCommand); if(res == 0){ #ifdef _BLE_TRACE_ Serial.print("Copying from command Response length: "); diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index 7d9c2c47..f7f93b84 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -144,6 +144,8 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t ATT.localKeyDistribution = KeyDistribution(pairingRequest->responderKeyDistribution); KeyDistribution rkd(pairingRequest->responderKeyDistribution); AuthReq req(pairingRequest->authReq); + KeyDistribution responseKD = KeyDistribution(); + responseKD.setIdKey(true); #ifdef _BLE_TRACE_ Serial.print("Req has properties: "); Serial.print(req.Bonding()?"bonding, ":"no bonding, "); @@ -171,7 +173,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t maxEncSize; uint8_t initiatorKeyDistribution; uint8_t responderKeyDistribution; - } response = { CONNECTION_PAIRING_RESPONSE, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 0x10, 0b1011, 0b1011}; + } response = { CONNECTION_PAIRING_RESPONSE, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 0x10, responseKD.getOctet(), responseKD.getOctet()}; HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); } @@ -316,9 +318,9 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t Serial.println("Calculate f5, f6:"); Serial.print("DH : "); btct.printBytes(HCI.DHKey,32); - Serial.println("Na : "); + Serial.print("Na : "); btct.printBytes(HCI.Na,16); - Serial.println("Nb : "); + Serial.print("Nb : "); btct.printBytes(HCI.Nb,16); Serial.print("MAC : "); btct.printBytes(MacKey,16);