Merge pull request #17 from unknownconstant/pairing-code

Pairing code & IOCaps set via implementation of callbacks

Author: unknownconstant

examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino

@@ -18,7 +18,7 @@
 #include <ArduinoBLE.h>
 
 
-#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.

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)
 {

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:

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),

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(){

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:
 

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;

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:

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 )

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);