diff --git a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino index fd40f5ee..dd3afbf6 100644 --- a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino +++ b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino @@ -18,7 +18,7 @@ #include -#define PAIR_BUTTON D3 // button for pairing +#define PAIR_BUTTON 3 // button for pairing #define PAIR_LED 24 // LED used to signal pairing #define PAIR_LED_ON LOW // Blue LED on Nano BLE has inverted logic #define PAIR_INTERVAL 30000 // interval for pairing after button press in ms @@ -42,6 +42,7 @@ int oldBatteryLevel = 0; // last battery level reading from analog input unsigned long previousMillis = 0; // last time the battery level was checked, in ms unsigned long pairingStarted = 0; // pairing start time when button is pressed bool wasConnected = 0; +bool acceptOrReject = true; void setup() { Serial.begin(9600); // initialize serial communication @@ -53,6 +54,30 @@ void setup() { Serial.println("Serial connected"); + + // Callback function with confirmation code when new device is pairing. + BLE.setDisplayCode([](uint32_t confirmCode){ + Serial.println("New device pairing request."); + Serial.print("Confirm code matches pairing device: "); + char code[6]; + sprintf(code, "%06d", confirmCode); + Serial.println(code); + }); + + // Callback to allow accepting or rejecting pairing + BLE.setBinaryConfirmPairing([&acceptOrReject](){ + Serial.print("Should we confirm pairing? "); + delay(5000); + if(acceptOrReject){ + acceptOrReject = false; + Serial.println("yes"); + return true; + }else{ + acceptOrReject = true; + Serial.println("no"); + return false; + } + }); // IRKs are keys that identify the true owner of a random mac address. // Add IRKs of devices you are bonded with. diff --git a/src/local/BLELocalDevice.cpp b/src/local/BLELocalDevice.cpp index 029fb488..d83c1cb3 100644 --- a/src/local/BLELocalDevice.cpp +++ b/src/local/BLELocalDevice.cpp @@ -434,6 +434,12 @@ void BLELocalDevice::setStoreLTK(int (*storeLTK)(uint8_t*, uint8_t*)){ void BLELocalDevice::setStoreIRK(int (*storeIRK)(uint8_t*, uint8_t*)){ HCI._storeIRK = storeIRK; } +void BLELocalDevice::setDisplayCode(void (*displayCode)(uint32_t confirmationCode)){ + HCI._displayCode = displayCode; +} +void BLELocalDevice::setBinaryConfirmPairing(bool (*binaryConfirmPairing)()){ + HCI._binaryConfirmPairing = binaryConfirmPairing; +} void BLELocalDevice::debug(Stream& stream) { diff --git a/src/local/BLELocalDevice.h b/src/local/BLELocalDevice.h index 75e7dc31..6c45c063 100644 --- a/src/local/BLELocalDevice.h +++ b/src/local/BLELocalDevice.h @@ -104,6 +104,9 @@ class BLELocalDevice { // address - The mac address needing its LTK // LTK - 16 octet LTK for the mac address virtual void setGetLTK(int (*getLTK)(uint8_t* address, uint8_t* LTK)); + + virtual void setDisplayCode(void (*displayCode)(uint32_t confirmationCode)); + virtual void setBinaryConfirmPairing(bool (*binaryConfirmPairing)()); uint8_t BDaddress[6]; protected: diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index a5166217..91ffeed0 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -81,6 +81,8 @@ #define ATT_ECODE_UNSUPP_GRP_TYPE 0x10 #define ATT_ECODE_INSUFF_RESOURCES 0x11 +// #define _BLE_TRACE_ + ATTClass::ATTClass() : _maxMtu(23), _timeout(5000), diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 9bcec4e1..82303ed3 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -26,7 +26,6 @@ #include "bitDescriptions.h" // #define _BLE_TRACE_ -//#define _BLE_TRACE_ #define HCI_COMMAND_PKT 0x01 #define HCI_ACLDATA_PKT 0x02 @@ -1258,6 +1257,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) uint8_t publicKey[64]; } pairingPublicKey = {CONNECTION_PAIRING_PUBLIC_KEY,0}; memcpy(pairingPublicKey.publicKey,evtReadLocalP256Complete->localPublicKey,64); + memcpy(localPublicKeyBuffer, evtReadLocalP256Complete->localPublicKey,64); // Send the local public key to the remote uint16_t connectionHandle = ATT.getPeerEncrptingConnectionHandle(); @@ -1456,6 +1456,19 @@ int HCIClass::storeLTK(uint8_t* address, uint8_t* LTK){ return 0; } } +uint8_t HCIClass::localIOCap(){ + if(_displayCode!=0){ + /// We have a display + if(_binaryConfirmPairing!=0){ + return IOCAP_DISPLAY_YES_NO; + }else{ + return IOCAP_DISPLAY_ONLY; + } + }else{ + // We have no display + return IOCAP_NO_INPUT_NO_OUTPUT; + } +} /// Stub function to generate parameters for local authreq AuthReq HCIClass::localAuthreq(){ diff --git a/src/utility/HCI.h b/src/utility/HCI.h index f2bc535e..af46265a 100644 --- a/src/utility/HCI.h +++ b/src/utility/HCI.h @@ -97,6 +97,7 @@ class HCIClass { // Generate a 64 bit random number virtual int leRand(uint8_t rand[]); virtual AuthReq localAuthreq(); + virtual uint8_t localIOCap(); virtual int saveNewAddress(uint8_t addressType, uint8_t* address, uint8_t* peerIrk, uint8_t* remoteIrk); virtual int leAddResolvingAddress(uint8_t addressType, uint8_t* address, uint8_t* peerIrk, uint8_t* remoteIrk); @@ -119,6 +120,7 @@ class HCIClass { // TODO: Send command be private again & use ATT implementation of send command within ATT. virtual int sendCommand(uint16_t opcode, uint8_t plen = 0, void* parameters = NULL); uint8_t remotePublicKeyBuffer[64]; + uint8_t localPublicKeyBuffer[64]; uint8_t remoteDHKeyCheckBuffer[16]; uint8_t Na[16]; uint8_t Nb[16]; @@ -132,6 +134,8 @@ class HCIClass { int (*_getIRKs)(uint8_t* nIRKs,uint8_t** BADDR_type, uint8_t*** BADDRs, uint8_t*** IRKs) = 0; int (*_storeLTK)(uint8_t*, uint8_t*) = 0; int (*_getLTK)(uint8_t*, uint8_t*) = 0; + void (*_displayCode)(uint32_t confirmationCode) = 0; + bool (*_binaryConfirmPairing)() = 0; private: diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index f7712051..2ef08b62 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -180,7 +180,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, HCI.localAuthreq().getOctet(), 0x10, responseKD.getOctet(), responseKD.getOctet()}; + } response = { CONNECTION_PAIRING_RESPONSE, HCI.localIOCap(), 0, HCI.localAuthreq().getOctet(), 0x10, responseKD.getOctet(), responseKD.getOctet()}; HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); @@ -209,6 +209,55 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t for(int i=0; i< 16; i++) response.Nb[15-i] = HCI.Nb[i]; HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); + + // We now have all needed for compare value + uint8_t g2Result[4]; + uint8_t U[32]; + uint8_t V[32]; + + for(int i=0; i<32; i++){ + U[31-i] = HCI.remotePublicKeyBuffer[i]; + V[31-i] = HCI.localPublicKeyBuffer[i]; + } + + btct.g2(U,V,HCI.Na,HCI.Nb, g2Result); + uint32_t result = 0; + for(int i=0; i<4; i++) result += g2Result[3-i] << 8*i; + +#ifdef _BLE_TRACE_ + Serial.print("U : "); + btct.printBytes(U,32); + Serial.print("V : "); + btct.printBytes(V,32); + Serial.print("X : "); + btct.printBytes(X,16); + Serial.print("Y : "); + btct.printBytes(Y,16); + Serial.print("g2res : "); + btct.printBytes(g2Result,4); + Serial.print("Result : "); + Serial.println(result); +#endif + + if(HCI._displayCode!=0){ + HCI._displayCode(result%1000000); + } + if(HCI._binaryConfirmPairing!=0){ + if(!HCI._binaryConfirmPairing()){ +#ifdef _BLE_TRACE_ + Serial.println("User rejection"); +#endif + uint8_t rejection[2]; + rejection[0] = CONNECTION_PAIRING_FAILED; + rejection[1] = 0x0C; // Numeric comparison failed + HCI.sendAclPkt(connectionHandle, SECURITY_CID, 2, rejection); + ATT.setPeerEncryption(connectionHandle, PEER_ENCRYPTION::NO_ENCRYPTION); + }else{ +#ifdef _BLE_TRACE_ + Serial.println("User did confirm"); +#endif + } + } } else if (code == CONNECTION_PAIRING_RESPONSE) { @@ -224,6 +273,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t Serial.print("Pairing failed with code: 0x"); Serial.println(pairingFailed->reason,HEX); #endif + ATT.setPeerEncryption(connectionHandle, PEER_ENCRYPTION::NO_ENCRYPTION); } else if (code == CONNECTION_IDENTITY_INFORMATION){ struct __attribute__ ((packed)) IdentityInformation { @@ -338,7 +388,7 @@ void L2CAPSignalingClass::smCalculateLTKandConfirm(uint16_t handle, uint8_t expe uint8_t Eb[16]; uint8_t R[16]; uint8_t MasterIOCap[3]; - uint8_t SlaveIOCap[3] = {HCI.localAuthreq().getOctet(), 0x0, LOCAL_IOCAP}; + uint8_t SlaveIOCap[3] = {HCI.localAuthreq().getOctet(), 0x0, HCI.localIOCap()}; ATT.getPeerIOCap(handle, MasterIOCap); for(int i=0; i<16; i++) R[i] = 0; diff --git a/src/utility/L2CAPSignaling.h b/src/utility/L2CAPSignaling.h index f8a361ee..26042167 100644 --- a/src/utility/L2CAPSignaling.h +++ b/src/utility/L2CAPSignaling.h @@ -49,7 +49,7 @@ #define LOCAL_AUTHREQ 0b00101101 -#define LOCAL_IOCAP IOCAP_NO_INPUT_NO_OUTPUT // will use JustWorks pairing +// #define LOCAL_IOCAP IOCAP_DISPLAY_ONLY // will use JustWorks pairing class L2CAPSignalingClass { public: diff --git a/src/utility/btct.cpp b/src/utility/btct.cpp index d78e328e..b4faf053 100644 --- a/src/utility/btct.cpp +++ b/src/utility/btct.cpp @@ -150,6 +150,42 @@ void BluetoothCryptoToolbox::testAh() printBytes(ourResult, 3); } +int BluetoothCryptoToolbox::g2(uint8_t U[], uint8_t V[], uint8_t X[], uint8_t Y[], uint8_t out[4]) +{ + struct __attribute__ ((packed)) CmacInput { + uint8_t U[32]; + uint8_t V[32]; + uint8_t Y[16]; + } cmacInput= {0,0,0}; + memcpy(cmacInput.U,U,32); + memcpy(cmacInput.V,V,32); + memcpy(cmacInput.Y,Y,16); + uint8_t intermediate[16]; + AES_CMAC(X,(uint8_t*)&cmacInput,sizeof(CmacInput),intermediate); + memcpy(out,&intermediate[12],4); + return 1; +} +void BluetoothCryptoToolbox::testg2(){ + uint8_t U[32] = {0x20,0xb0,0x03,0xd2,0xf2,0x97,0xbe,0x2c,0x5e,0x2c,0x83,0xa7,0xe9,0xf9,0xa5,0xb9,0xef,0xf4,0x91,0x11,0xac,0xf4,0xfd,0xdb,0xcc,0x03,0x01,0x48,0x0e,0x35,0x9d,0xe6}; + uint8_t V[32] = {0x55,0x18,0x8b,0x3d,0x32,0xf6,0xbb,0x9a,0x90,0x0a,0xfc,0xfb,0xee,0xd4,0xe7,0x2a,0x59,0xcb,0x9a,0xc2,0xf1,0x9d,0x7c,0xfb,0x6b,0x4f,0xdd,0x49,0xf4,0x7f,0xc5,0xfd}; + uint8_t X[16] = {0xd5,0xcb,0x84,0x54,0xd1,0x77,0x73,0x3e,0xff,0xff,0xb2,0xec,0x71,0x2b,0xae,0xab}; + uint8_t Y[16] = {0xa6,0xe8,0xe7,0xcc,0x25,0xa7,0x5f,0x6e,0x21,0x65,0x83,0xf7,0xff,0x3d,0xc4,0xcf}; + uint8_t AES[16] = {0x15,0x36,0xd1,0x8d,0xe3,0xd2,0x0d,0xf9,0x9b,0x70,0x44,0xc1,0x2f,0x9e,0xd5,0xba}; + uint8_t out[4]; + + + uint32_t expected = 0; + g2(U,V,X,Y,out); + uint32_t result = 0; + for(int i=0; i<4; i++) result += out[i] << 8*i; + + Serial.print("Expected : "); + Serial.println(expected); + Serial.print("Result : "); + Serial.println(result); + Serial.println(); + +} void BluetoothCryptoToolbox::AES_CMAC ( unsigned char *key, unsigned char *input, int length, unsigned char *mac ) diff --git a/src/utility/btct.h b/src/utility/btct.h index 43c0b0d6..08f8f192 100644 --- a/src/utility/btct.h +++ b/src/utility/btct.h @@ -13,11 +13,13 @@ class BluetoothCryptoToolbox{ int f5(uint8_t DHKey[],uint8_t N_master[], uint8_t N_slave[], uint8_t BD_ADDR_master[], uint8_t BD_ADDR_slave[], uint8_t MacKey[], uint8_t LTK[]); int f6(uint8_t W[], uint8_t N1[],uint8_t N2[],uint8_t R[], uint8_t IOCap[], uint8_t A1[], uint8_t A2[], uint8_t Ex[]); + int g2(uint8_t U[], uint8_t V[], uint8_t X[], uint8_t Y[], uint8_t out[4]); int ah(uint8_t k[16], uint8_t r[3], uint8_t result[3]); void test(); void testF5(); void testF6(); void testAh(); + void testg2(); private: int AES_128(uint8_t key[], uint8_t data_in[], uint8_t data_out[]); void leftshift_onebit(unsigned char *input,unsigned char *output);