From fa42bb26cf49216b359999df944df4b868ae137a Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Sat, 2 Jan 2021 20:15:00 +0000 Subject: [PATCH] Write encryption & visible LTK / IRK --- .../EncryptedBatteryMonitor.ino | 209 ++++++++++++ src/BLECharacteristic.cpp | 8 +- src/BLECharacteristic.h | 4 +- src/BLEProperty.h | 11 +- src/BLETypedCharacteristic.h | 6 +- src/BLETypedCharacteristics.cpp | 28 +- src/BLETypedCharacteristics.h | 28 +- src/local/BLELocalCharacteristic.cpp | 18 +- src/local/BLELocalCharacteristic.h | 6 +- src/local/BLELocalDevice.cpp | 69 +++- src/local/BLELocalDevice.h | 7 +- src/remote/BLERemoteCharacteristic.cpp | 5 +- src/remote/BLERemoteCharacteristic.h | 4 +- src/utility/ATT.cpp | 45 ++- src/utility/ATT.h | 19 +- src/utility/HCI.cpp | 315 +++++++++++++++++- src/utility/HCI.h | 17 + src/utility/L2CAPSignaling.cpp | 45 ++- src/utility/btct.cpp | 32 ++ src/utility/btct.h | 2 + src/utility/keyDistribution.cpp | 24 ++ src/utility/keyDistribution.h | 29 ++ 22 files changed, 855 insertions(+), 76 deletions(-) create mode 100644 examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino create mode 100644 src/utility/keyDistribution.cpp create mode 100644 src/utility/keyDistribution.h diff --git a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino new file mode 100644 index 00000000..338a374e --- /dev/null +++ b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino @@ -0,0 +1,209 @@ +/* + Battery Monitor + + This example creates a BLE peripheral with the standard battery service and + level characteristic. The A0 pin is used to calculate the battery level. + + The circuit: + - Arduino MKR WiFi 1010, Arduino Uno WiFi Rev2 board, Arduino Nano 33 IoT, + Arduino Nano 33 BLE, or Arduino Nano 33 BLE Sense board. + + You can use a generic BLE central app, like LightBlue (iOS and Android) or + nRF Connect (Android), to interact with the services and characteristics + created in this sketch. + + This example code is in the public domain. +*/ + +#include + + + // BLE Battery Service +BLEService batteryService("180F"); + +// BLE Battery Level Characteristic +BLEUnsignedCharCharacteristic batteryLevelChar("2A19", // standard 16-bit characteristic UUID + BLERead | BLENotify); // remote clients will be able to get notifications if this characteristic changes +BLEStringCharacteristic stringcharacteristic("183E", BLERead | BLEWrite, 31); + + +// Add BLEEncryption tag to require pairing. This controls the LED. +BLEUnsignedCharCharacteristic secretValue("2a3F", BLERead | BLEWrite | BLEEncryption); + +int oldBatteryLevel = 0; // last battery level reading from analog input +long previousMillis = 0; // last time the battery level was checked, in ms + +void setup() { + Serial.begin(9600); // initialize serial communication + while (!Serial); + + pinMode(LED_BUILTIN, OUTPUT); // initialize the built-in LED pin to indicate when a central is connected + + + Serial.println("Serial connected"); + + // 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){ + // Set to number of devices + *nIRKs = 2; + + *BDAddrs = new uint8_t*[*nIRKs]; + *IRKs = new uint8_t*[*nIRKs]; + *BADDR_TYPES = 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 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, }; + + + (*BADDR_TYPES)[0] = 0; + (*IRKs)[0] = new uint8_t[16]; + memcpy((*IRKs)[0],iPhoneIRK,16); + (*BDAddrs)[0] = new uint8_t[6]; + memcpy((*BDAddrs)[0], iPhoneMac, 6); + + + (*BADDR_TYPES)[1] = 0; + (*IRKs)[1] = new uint8_t[16]; + memcpy((*IRKs)[1],iPadIRK,16); + (*BDAddrs)[1] = new uint8_t[6]; + memcpy((*BDAddrs)[1], iPadMac, 6); + + + return 1; + }); + // The LTK is the secret key which is used to encrypt bluetooth traffic + BLE.setGetLTK([](uint8_t* address, uint8_t* LTK){ + // address is input + Serial.print("Recieved request for address: "); + 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}; + + + if(memcmp(iPhoneMac, address, 6)==0){ + memcpy(LTK, iPhoneLTK, 16); + return 1; + }else if(memcmp(iPadMac, address, 6)==0){ + memcpy(LTK, iPadLTK, 16); + } + return 0; + }); + BLE.setStoreIRK([](uint8_t* address, uint8_t* IRK){ + Serial.print(F("New device with MAC : ")); + btct.printBytes(address,6); + Serial.print(F("Need to store IRK : ")); + btct.printBytes(IRK,16); + return 1; + }); + BLE.setStoreLTK([](uint8_t* address, uint8_t* LTK){ + Serial.print(F("New device with MAC : ")); + btct.printBytes(address,6); + Serial.print(F("Need to store LTK : ")); + btct.printBytes(LTK,16); + return 1; + }); + + while(1){// begin initialization + if (!BLE.begin()) { + Serial.println("starting BLE failed!"); + delay(200); + continue; + } + Serial.println("BT init"); + delay(200); + + /* Set a local name for the BLE device + This name will appear in advertising packets + and can be used by remote devices to identify this BLE device + The name can be changed but maybe be truncated based on space left in advertisement packet + */ + + BLE.setDeviceName("Arduino"); + BLE.setLocalName("BatteryMonitor"); + + BLE.setAdvertisedService(batteryService); // add the service UUID + batteryService.addCharacteristic(batteryLevelChar); // add the battery level characteristic + batteryService.addCharacteristic(stringcharacteristic); + batteryService.addCharacteristic(secretValue); + + BLE.addService(batteryService); // Add the battery service + batteryLevelChar.writeValue(oldBatteryLevel); // set initial value for this characteristic + char* stringCharValue = new char[32]; + stringCharValue = "string"; + stringcharacteristic.writeValue(stringCharValue); + secretValue.writeValue(0); + + delay(1000); + + /* Start advertising BLE. It will start continuously transmitting BLE + advertising packets and will be visible to remote BLE central devices + until it receives a new connection */ + + // start advertising + if(!BLE.advertise()){ + Serial.println("failed to advertise bluetooth."); + BLE.stopAdvertise(); + delay(500); + }else{ + Serial.println("advertising..."); + break; + } + BLE.end(); + delay(100); + } +} + + +void loop() { + // wait for a BLE central + BLEDevice central = BLE.central(); + + // if a central is connected to the peripheral: + if (central) { + Serial.print("Connected to central: "); + // print the central's BT address: + Serial.println(central.address()); + + // check the battery level every 200ms + // while the central is connected: + while (central.connected()) { + long currentMillis = millis(); + // if 200ms have passed, check the battery level: + if (currentMillis - previousMillis >= 1000) { + previousMillis = currentMillis; + updateBatteryLevel(); + if(secretValue.value()>0){ + digitalWrite(13,HIGH); + }else{ + digitalWrite(13,LOW); + } + } + } + Serial.print("Disconnected from central: "); + Serial.println(central.address()); + } +} + +void updateBatteryLevel() { + /* Read the current voltage level on the A0 analog input pin. + This is used here to simulate the charge level of a battery. + */ + int battery = analogRead(A0); + int batteryLevel = map(battery, 0, 1023, 0, 100); + + if (batteryLevel != oldBatteryLevel) { // if the battery level has changed + // Serial.print("Battery Level % is now: "); // print it + // Serial.println(batteryLevel); + batteryLevelChar.writeValue(batteryLevel); // and update the battery level characteristic + oldBatteryLevel = batteryLevel; // save the level for next comparison + } +} diff --git a/src/BLECharacteristic.cpp b/src/BLECharacteristic.cpp index 9a07cb9d..1cfbf489 100644 --- a/src/BLECharacteristic.cpp +++ b/src/BLECharacteristic.cpp @@ -47,13 +47,13 @@ BLECharacteristic::BLECharacteristic(BLERemoteCharacteristic* remote) : } } -BLECharacteristic::BLECharacteristic(const char* uuid, uint8_t properties, int valueSize, bool fixedLength) : - BLECharacteristic(new BLELocalCharacteristic(uuid, properties, valueSize, fixedLength)) +BLECharacteristic::BLECharacteristic(const char* uuid, uint16_t permissions, int valueSize, bool fixedLength) : + BLECharacteristic(new BLELocalCharacteristic(uuid, permissions, valueSize, fixedLength)) { } -BLECharacteristic::BLECharacteristic(const char* uuid, uint8_t properties, const char* value) : - BLECharacteristic(new BLELocalCharacteristic(uuid, properties, value)) +BLECharacteristic::BLECharacteristic(const char* uuid, uint16_t permissions, const char* value) : + BLECharacteristic(new BLELocalCharacteristic(uuid, permissions, value)) { } diff --git a/src/BLECharacteristic.h b/src/BLECharacteristic.h index 2ac30b05..da9721e0 100644 --- a/src/BLECharacteristic.h +++ b/src/BLECharacteristic.h @@ -45,8 +45,8 @@ class BLERemoteCharacteristic; class BLECharacteristic { public: BLECharacteristic(); - BLECharacteristic(const char* uuid, uint8_t properties, int valueSize, bool fixedLength = false); - BLECharacteristic(const char* uuid, uint8_t properties, const char* value); + BLECharacteristic(const char* uuid, uint16_t permissions, int valueSize, bool fixedLength = false); + BLECharacteristic(const char* uuid, uint16_t permissions, const char* value); BLECharacteristic(const BLECharacteristic& other); virtual ~BLECharacteristic(); diff --git a/src/BLEProperty.h b/src/BLEProperty.h index aeee4c85..434bd2aa 100644 --- a/src/BLEProperty.h +++ b/src/BLEProperty.h @@ -29,10 +29,19 @@ enum BLEProperty { BLEWrite = 0x08, BLENotify = 0x10, BLEIndicate = 0x20, - BLEAuth = 1 << 6, + BLEAuthSignedWrite = 1 << 6, BLEExtProp = 1 << 7, }; +enum BLEPermission { + BLEEncryption = 1 << 9, + BLEAuthentication = 1 << 10, + BLEAuthorization = 1 << 11, + // BLEWriteEncryption = 1 << 11, + // BLEWriteAuthentication = 1 << 12, + // BLEWriteAuthorization = 1 << 13, +}; + #define ESP_GATT_CHAR_PROP_BIT_BROADCAST (1 << 0) /* 0x01 */ /* relate to BTA_GATT_CHAR_PROP_BIT_BROADCAST in bta/bta_gatt_api.h */ #define ESP_GATT_CHAR_PROP_BIT_READ (1 << 1) /* 0x02 */ /* relate to BTA_GATT_CHAR_PROP_BIT_READ in bta/bta_gatt_api.h */ #define ESP_GATT_CHAR_PROP_BIT_WRITE_NR (1 << 2) /* 0x04 */ /* relate to BTA_GATT_CHAR_PROP_BIT_WRITE_NR in bta/bta_gatt_api.h */ diff --git a/src/BLETypedCharacteristic.h b/src/BLETypedCharacteristic.h index d6e6e4a1..7777d360 100644 --- a/src/BLETypedCharacteristic.h +++ b/src/BLETypedCharacteristic.h @@ -25,7 +25,7 @@ template class BLETypedCharacteristic : public BLECharacteristic { public: - BLETypedCharacteristic(const char* uuid, unsigned char properties); + BLETypedCharacteristic(const char* uuid, unsigned int permissions); int writeValue(T value); int setValue(T value) { return writeValue(value); } @@ -43,8 +43,8 @@ template class BLETypedCharacteristic : public BLECharacteristic T byteSwap(T value); }; -template BLETypedCharacteristic::BLETypedCharacteristic(const char* uuid, unsigned char properties) : - BLECharacteristic(uuid, properties, sizeof(T), true) +template BLETypedCharacteristic::BLETypedCharacteristic(const char* uuid, unsigned int permissions) : + BLECharacteristic(uuid, permissions, sizeof(T), true) { T value; memset(&value, 0x00, sizeof(value)); diff --git a/src/BLETypedCharacteristics.cpp b/src/BLETypedCharacteristics.cpp index 976f6159..800574eb 100644 --- a/src/BLETypedCharacteristics.cpp +++ b/src/BLETypedCharacteristics.cpp @@ -21,72 +21,72 @@ #include "BLETypedCharacteristics.h" -BLEBoolCharacteristic::BLEBoolCharacteristic(const char* uuid, unsigned char properties) : +BLEBoolCharacteristic::BLEBoolCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEBooleanCharacteristic::BLEBooleanCharacteristic(const char* uuid, unsigned char properties) : +BLEBooleanCharacteristic::BLEBooleanCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLECharCharacteristic::BLECharCharacteristic(const char* uuid, unsigned char properties) : +BLECharCharacteristic::BLECharCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEUnsignedCharCharacteristic::BLEUnsignedCharCharacteristic(const char* uuid, unsigned char properties) : +BLEUnsignedCharCharacteristic::BLEUnsignedCharCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEByteCharacteristic::BLEByteCharacteristic(const char* uuid, unsigned char properties) : +BLEByteCharacteristic::BLEByteCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEShortCharacteristic::BLEShortCharacteristic(const char* uuid, unsigned char properties) : +BLEShortCharacteristic::BLEShortCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEUnsignedShortCharacteristic::BLEUnsignedShortCharacteristic(const char* uuid, unsigned char properties) : +BLEUnsignedShortCharacteristic::BLEUnsignedShortCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEWordCharacteristic::BLEWordCharacteristic(const char* uuid, unsigned char properties) : +BLEWordCharacteristic::BLEWordCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEIntCharacteristic::BLEIntCharacteristic(const char* uuid, unsigned char properties) : +BLEIntCharacteristic::BLEIntCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEUnsignedIntCharacteristic::BLEUnsignedIntCharacteristic(const char* uuid, unsigned char properties) : +BLEUnsignedIntCharacteristic::BLEUnsignedIntCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLELongCharacteristic::BLELongCharacteristic(const char* uuid, unsigned char properties) : +BLELongCharacteristic::BLELongCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEUnsignedLongCharacteristic::BLEUnsignedLongCharacteristic(const char* uuid, unsigned char properties) : +BLEUnsignedLongCharacteristic::BLEUnsignedLongCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEFloatCharacteristic::BLEFloatCharacteristic(const char* uuid, unsigned char properties) : +BLEFloatCharacteristic::BLEFloatCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEDoubleCharacteristic::BLEDoubleCharacteristic(const char* uuid, unsigned char properties) : +BLEDoubleCharacteristic::BLEDoubleCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } diff --git a/src/BLETypedCharacteristics.h b/src/BLETypedCharacteristics.h index 465fc046..df8482cf 100644 --- a/src/BLETypedCharacteristics.h +++ b/src/BLETypedCharacteristics.h @@ -24,72 +24,72 @@ class BLEBoolCharacteristic : public BLETypedCharacteristic { public: - BLEBoolCharacteristic(const char* uuid, unsigned char properties); + BLEBoolCharacteristic(const char* uuid, unsigned int permissions); }; class BLEBooleanCharacteristic : public BLETypedCharacteristic { public: - BLEBooleanCharacteristic(const char* uuid, unsigned char properties); + BLEBooleanCharacteristic(const char* uuid, unsigned int permissions); }; class BLECharCharacteristic : public BLETypedCharacteristic { public: - BLECharCharacteristic(const char* uuid, unsigned char properties); + BLECharCharacteristic(const char* uuid, unsigned int permissions); }; class BLEUnsignedCharCharacteristic : public BLETypedCharacteristic { public: - BLEUnsignedCharCharacteristic(const char* uuid, unsigned char properties); + BLEUnsignedCharCharacteristic(const char* uuid, unsigned int permissions); }; class BLEByteCharacteristic : public BLETypedCharacteristic { public: - BLEByteCharacteristic(const char* uuid, unsigned char properties); + BLEByteCharacteristic(const char* uuid, unsigned int permissions); }; class BLEShortCharacteristic : public BLETypedCharacteristic { public: - BLEShortCharacteristic(const char* uuid, unsigned char properties); + BLEShortCharacteristic(const char* uuid, unsigned int permissions); }; class BLEUnsignedShortCharacteristic : public BLETypedCharacteristic { public: - BLEUnsignedShortCharacteristic(const char* uuid, unsigned char properties); + BLEUnsignedShortCharacteristic(const char* uuid, unsigned int permissions); }; class BLEWordCharacteristic : public BLETypedCharacteristic { public: - BLEWordCharacteristic(const char* uuid, unsigned char properties); + BLEWordCharacteristic(const char* uuid, unsigned int permissions); }; class BLEIntCharacteristic : public BLETypedCharacteristic { public: - BLEIntCharacteristic(const char* uuid, unsigned char properties); + BLEIntCharacteristic(const char* uuid, unsigned int permissions); }; class BLEUnsignedIntCharacteristic : public BLETypedCharacteristic { public: - BLEUnsignedIntCharacteristic(const char* uuid, unsigned char properties); + BLEUnsignedIntCharacteristic(const char* uuid, unsigned int permissions); }; class BLELongCharacteristic : public BLETypedCharacteristic { public: - BLELongCharacteristic(const char* uuid, unsigned char properties); + BLELongCharacteristic(const char* uuid, unsigned int permissions); }; class BLEUnsignedLongCharacteristic : public BLETypedCharacteristic { public: - BLEUnsignedLongCharacteristic(const char* uuid, unsigned char properties); + BLEUnsignedLongCharacteristic(const char* uuid, unsigned int permissions); }; class BLEFloatCharacteristic : public BLETypedCharacteristic { public: - BLEFloatCharacteristic(const char* uuid, unsigned char properties); + BLEFloatCharacteristic(const char* uuid, unsigned int permissions); }; class BLEDoubleCharacteristic : public BLETypedCharacteristic { public: - BLEDoubleCharacteristic(const char* uuid, unsigned char properties); + BLEDoubleCharacteristic(const char* uuid, unsigned int permissions); }; #endif diff --git a/src/local/BLELocalCharacteristic.cpp b/src/local/BLELocalCharacteristic.cpp index 333d00b2..2cd801b2 100644 --- a/src/local/BLELocalCharacteristic.cpp +++ b/src/local/BLELocalCharacteristic.cpp @@ -29,20 +29,21 @@ #include "BLELocalCharacteristic.h" -BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint8_t properties, int valueSize, bool fixedLength) : +BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint16_t permissions, int valueSize, bool fixedLength) : BLELocalAttribute(uuid), - _properties(properties), + _properties((uint8_t)(permissions&0x000FF)), _valueSize(min(valueSize, 512)), _valueLength(0), _fixedLength(fixedLength), _handle(0x0000), _broadcast(false), _written(false), - _cccdValue(0x0000) + _cccdValue(0x0000), + _permissions((uint8_t)((permissions&0xFF00)>>8)) { memset(_eventHandlers, 0x00, sizeof(_eventHandlers)); - if (properties & (BLENotify | BLEIndicate)) { + if (permissions & (BLENotify | BLEIndicate)) { BLELocalDescriptor* cccd = new BLELocalDescriptor("2902", (uint8_t*)&_cccdValue, sizeof(_cccdValue)); _descriptors.add(cccd); @@ -51,12 +52,11 @@ BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint8_t propert _value = (uint8_t*)malloc(valueSize); } -BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint8_t properties, const char* value) : - BLELocalCharacteristic(uuid, properties, strlen(value)) +BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint16_t permissions, const char* value) : + BLELocalCharacteristic(uuid, permissions, strlen(value)) { writeValue(value); } - BLELocalCharacteristic::~BLELocalCharacteristic() { for (unsigned int i = 0; i < descriptorCount(); i++) { @@ -84,6 +84,10 @@ uint8_t BLELocalCharacteristic::properties() const return _properties; } +uint8_t BLELocalCharacteristic::permissions() const { + return _permissions; +} + int BLELocalCharacteristic::valueSize() const { return _valueSize; diff --git a/src/local/BLELocalCharacteristic.h b/src/local/BLELocalCharacteristic.h index ee42390a..331cdd5c 100644 --- a/src/local/BLELocalCharacteristic.h +++ b/src/local/BLELocalCharacteristic.h @@ -33,13 +33,14 @@ class BLELocalDescriptor; class BLELocalCharacteristic : public BLELocalAttribute { public: - BLELocalCharacteristic(const char* uuid, uint8_t properties, int valueSize, bool fixedLength = false); - BLELocalCharacteristic(const char* uuid, uint8_t properties, const char* value); + BLELocalCharacteristic(const char* uuid, uint16_t permissions, int valueSize, bool fixedLength = false); + BLELocalCharacteristic(const char* uuid, uint16_t permissions, const char* value); virtual ~BLELocalCharacteristic(); virtual enum BLEAttributeType type() const; uint8_t properties() const; + uint8_t permissions() const; int valueSize() const; const uint8_t* value() const; @@ -75,6 +76,7 @@ class BLELocalCharacteristic : public BLELocalAttribute { private: uint8_t _properties; + uint8_t _permissions; int _valueSize; uint8_t* _value; uint16_t _valueLength; diff --git a/src/local/BLELocalDevice.cpp b/src/local/BLELocalDevice.cpp index df11e971..49cc6603 100644 --- a/src/local/BLELocalDevice.cpp +++ b/src/local/BLELocalDevice.cpp @@ -108,7 +108,7 @@ int BLELocalDevice::begin() end(); return 0; } - if (HCI.setLeEventMask(0x00000000000001FF) != 0) { + if (HCI.setLeEventMask(0x00000000000003FF) != 0) { end(); return 0; } @@ -121,6 +121,59 @@ int BLELocalDevice::begin() return 0; } + /// The HCI should allow automatic address resolution. + + // // If we have callbacks to rememember bonded devices: + // if(HCI._getIRKs!=0){ + // uint8_t nIRKs = 0; + // uint8_t** BADDR_Type = new uint8_t*; + // uint8_t*** BADDRs = new uint8_t**; + // uint8_t*** IRKs = new uint8_t**; + // uint8_t* memcheck; + + + // if(!HCI._getIRKs(&nIRKs, BADDR_Type, BADDRs, IRKs)){ + // Serial.println("error"); + // } + // for(int i=0; i>8)), _valueHandle(valueHandle), _value(NULL), _valueLength(0), diff --git a/src/remote/BLERemoteCharacteristic.h b/src/remote/BLERemoteCharacteristic.h index d0ac09bc..b53ab031 100644 --- a/src/remote/BLERemoteCharacteristic.h +++ b/src/remote/BLERemoteCharacteristic.h @@ -29,10 +29,11 @@ class BLERemoteCharacteristic : public BLERemoteAttribute { public: - BLERemoteCharacteristic(const uint8_t uuid[], uint8_t uuidLen, uint16_t connectionHandle, uint16_t startHandle, uint8_t properties, uint16_t valueHandle); + BLERemoteCharacteristic(const uint8_t uuid[], uint8_t uuidLen, uint16_t connectionHandle, uint16_t startHandle, uint16_t permissions, uint16_t valueHandle); virtual ~BLERemoteCharacteristic(); uint8_t properties() const; + uint8_t permissions() const; const uint8_t* value() const; int valueLength() const; @@ -66,6 +67,7 @@ class BLERemoteCharacteristic : public BLERemoteAttribute { uint16_t _connectionHandle; uint16_t _startHandle; uint8_t _properties; + uint8_t _permissions; uint16_t _valueHandle; uint8_t* _value; diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index 5bd5e2aa..6ef6bc31 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -253,6 +253,15 @@ void ATTClass::addConnection(uint16_t handle, uint8_t role, uint8_t peerBdaddrTy _peers[peerIndex].mtu = 23; _peers[peerIndex].addressType = peerBdaddrType; memcpy(_peers[peerIndex].address, peerBdaddr, sizeof(_peers[peerIndex].address)); + uint8_t BDADDr[6]; + for(int i=0; i<6; i++) BDADDr[5-i] = peerBdaddr[i]; + if(HCI.tryResolveAddress(BDADDr,_peers[peerIndex].resolvedAddress)){ +#ifdef _BLE_TRACE_ + Serial.println("Found match."); +#endif + }else{ + memset(_peers[peerIndex].resolvedAddress, 0, 6); + } if (_eventHandlers[BLEConnected]) { _eventHandlers[BLEConnected](BLEDevice(peerBdaddrType, peerBdaddr)); @@ -515,6 +524,7 @@ bool ATTClass::disconnect() _peers[i].role = 0x00; _peers[i].addressType = 0x00; memset(_peers[i].address, 0x00, sizeof(_peers[i].address)); + memset(_peers[i].resolvedAddress, 0x00, sizeof(_peers[i].resolvedAddress)); _peers[i].mtu = 23; if (_peers[i].device) { @@ -1007,7 +1017,8 @@ void ATTClass::readOrReadBlobReq(uint16_t connectionHandle, uint16_t mtu, uint8_ return; } // If characteristic requires encryption send error & hold response until encrypted - if ((characteristic->properties() & BLEAuth) > 0 && (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES)==0) { + if ((characteristic->permissions() & (BLEPermission::BLEEncryption >> 8)) > 0 && + (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES)==0 ) { holdResponse = true; sendError(connectionHandle, opcode, handle, ATT_ECODE_INSUFF_ENC); } @@ -1231,7 +1242,8 @@ void ATTClass::writeReqOrCmd(uint16_t connectionHandle, uint16_t mtu, uint8_t op return; } // Check permssion - if((characteristic->properties() & BLEProperty::BLEAuth)> 0 && (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES) == 0){ + if((characteristic->permissions() &( BLEPermission::BLEEncryption >> 8)) > 0 && + (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES) == 0){ holdResponse = true; sendError(connectionHandle, ATT_OP_WRITE_REQ, handle, ATT_ECODE_INSUFF_ENC); } @@ -1817,7 +1829,7 @@ int ATTClass::setPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[3]){ // Return the connection handle for the first peer that is requesting encryption uint16_t ATTClass::getPeerEncrptingConnectionHandle(){ for(int i=0; i 0){ + if((_peers[i].encryption & PEER_ENCRYPTION::REQUESTED_ENCRYPTION) > 0){ return _peers[i].connectionHandle; } } @@ -1861,7 +1873,7 @@ int ATTClass::getPeerAddrWithType(uint16_t connectionHandle, uint8_t peerAddr[]) peerAddr[6-k] = _peers[i].address[k]; } if(_peers[i].addressType){ - peerAddr[0] = 0x01; + peerAddr[0] = _peers[i].addressType; }else{ peerAddr[0] = 0x00; } @@ -1869,6 +1881,31 @@ int ATTClass::getPeerAddrWithType(uint16_t connectionHandle, uint8_t peerAddr[]) } return 0; } +// Get the resolved address for a peer if it exists +int ATTClass::getPeerResolvedAddress(uint16_t connectionHandle, uint8_t resolvedAddress[]){ + for(int i=0; i #include "BLEDevice.h" +#include "keyDistribution.h" #define ATT_CID 0x0004 #define BLE_CTL 0x0008 @@ -37,11 +38,12 @@ enum PEER_ENCRYPTION { NO_ENCRYPTION = 0, - REQUESTED_ENCRYPTION = 1 << 0, - SENT_PUBKEY = 1 << 1, - DH_KEY_CALULATED = 1 << 2, - RECEIVED_DH_CHECK = 1 << 3, - SENT_DH_CHECK = 1 << 4, + PAIRING_REQUEST = 1 << 0, + REQUESTED_ENCRYPTION = 1 << 1, + SENT_PUBKEY = 1 << 2, + DH_KEY_CALULATED = 1 << 3, + RECEIVED_DH_CHECK = 1 << 4, + SENT_DH_CHECK = 1 << 5, ENCRYPTED_AES = 1 << 7 }; @@ -94,11 +96,17 @@ class ATTClass { virtual int getPeerAddrWithType(uint16_t connectionHandle, uint8_t peerAddr[]); virtual int setPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[]); virtual int getPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[]); + virtual int getPeerResolvedAddress(uint16_t connectionHandle, uint8_t* resolvedAddress); uint8_t holdBuffer[64]; uint8_t writeBuffer[64]; uint8_t holdBufferSize; uint8_t writeBufferSize; virtual int processWriteBuffer(); + KeyDistribution remoteKeyDistribution; + KeyDistribution localKeyDistribution; + uint8_t peerIRK[16]; + /// This is just a random number... Not sure it has use unless privacy mode is active. + uint8_t localIRK[16] = {0x54,0x83,0x63,0x7c,0xc5,0x1e,0xf7,0xec,0x32,0xdd,0xad,0x51,0x89,0x4b,0x9e,0x07}; private: virtual void error(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); virtual void mtuReq(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); @@ -139,6 +147,7 @@ class ATTClass { uint8_t role; uint8_t addressType; uint8_t address[6]; + uint8_t resolvedAddress[6]; uint16_t mtu; BLERemoteDevice* device; uint8_t encryption; diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 805a2c3d..c3d66848 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -34,6 +34,7 @@ #define EVT_CMD_COMPLETE 0x0e #define EVT_CMD_STATUS 0x0f #define EVT_NUM_COMP_PKTS 0x13 +#define EVT_RETURN_LINK_KEYS 0x15 #define EVT_UNKNOWN 0x10 #define EVT_LE_META_EVENT 0x3e @@ -449,6 +450,161 @@ int HCIClass::leConnUpdate(uint16_t handle, uint16_t minInterval, uint16_t maxIn return sendCommand(OGF_LE_CTL << 10 | OCF_LE_CONN_UPDATE, sizeof(leConnUpdateData), &leConnUpdateData); } +int HCIClass::saveNewAddress(uint8_t addressType, uint8_t* address, uint8_t* peerIrk, uint8_t* localIrk){ + if(_storeIRK!=0){ + _storeIRK(address, peerIrk); + } + // Again... this should work + // leAddResolvingAddress(addressType, address, peerIrk, localIrk); +} +int HCIClass::leAddResolvingAddress(uint8_t addressType, uint8_t* peerAddress, uint8_t* peerIrk, uint8_t* localIrk){ + leStopResolvingAddresses(); + + struct __attribute__ ((packed)) AddDevice { + uint8_t peerAddressType; + uint8_t peerAddress[6]; + uint8_t peerIRK[16]; + uint8_t localIRK[16]; + } addDevice; + addDevice.peerAddressType = addressType; + for(int i=0; i<6; i++) addDevice.peerAddress[5-i] = peerAddress[i]; + for(int i=0; i<16; i++) { + addDevice.peerIRK[15-i] = peerIrk[i]; + addDevice.localIRK[15-i] = localIrk[i]; + } + Serial.print("ADDTYPE :"); + btct.printBytes(&addDevice.peerAddressType,1); + Serial.print("adddddd :"); + btct.printBytes(addDevice.peerAddress,6); + Serial.print("Peer IRK :"); + btct.printBytes(addDevice.peerIRK,16); + Serial.print("localIRK :"); + btct.printBytes(addDevice.localIRK,16); + sendCommand(OGF_LE_CTL << 10 | 0x27, sizeof(addDevice), &addDevice); + + leStartResolvingAddresses(); +} +int HCIClass::leStopResolvingAddresses(){ + uint8_t enable = 0; + return HCI.sendCommand(OGF_LE_CTL << 10 | 0x2D, 1,&enable); // Disable address resolution +} +int HCIClass::leStartResolvingAddresses(){ + uint8_t enable = 1; + return HCI.sendCommand(OGF_LE_CTL << 10 | 0x2D, 1,&enable); // Disable address resolution +} +int HCIClass::leReadPeerResolvableAddress(uint8_t peerAddressType, uint8_t* peerIdentityAddress, uint8_t* peerResolvableAddress){ + struct __attribute__ ((packed)) Request { + uint8_t addressType; + uint8_t identityAddress[6]; + } request; + request.addressType = peerAddressType; + for(int i=0; i<6; i++) request.identityAddress[5-i] = peerIdentityAddress[i]; + + + int res = sendCommand(OGF_LE_CTL << 10 | 0x2B, sizeof(request), &request); + Serial.print("res: 0x"); + Serial.println(res, HEX); + if(res==0){ + struct __attribute__ ((packed)) Response { + uint8_t status; + uint8_t peerResolvableAddress[6]; + } *response = (Response*)_cmdResponse; + Serial.print("Address resolution status: 0x"); + Serial.println(response->status, HEX); + Serial.print("peer resolvable address: "); + btct.printBytes(response->peerResolvableAddress,6); + } + return res; +} + +int HCIClass::writeLK(uint8_t peerAddress[], uint8_t LK[]){ + struct __attribute__ ((packed)) StoreLK { + uint8_t nKeys; + uint8_t BD_ADDR[6]; + uint8_t LTK[16]; + } storeLK; + storeLK.nKeys = 1; + memcpy(storeLK.BD_ADDR, peerAddress, 6); + for(int i=0; i<16; i++) storeLK.LTK[15-i] = LK[i]; + HCI.sendCommand(OGF_HOST_CTL << 10 | 0x11, sizeof(storeLK), &storeLK); +} +int HCIClass::readStoredLKs(){ + uint8_t BD_ADDR[6]; + readStoredLK(BD_ADDR, 1); +} +int HCIClass::readStoredLK(uint8_t BD_ADDR[], uint8_t read_all ){ + struct __attribute__ ((packed)) Request { + uint8_t BD_ADDR[6]; + uint8_t read_a; + } request = {0,0}; + for(int i=0; i<6; i++) request.BD_ADDR[5-i] = BD_ADDR[i]; + request.read_a = read_all; + return sendCommand(OGF_HOST_CTL << 10 | 0xD, sizeof(request), &request); +} + +int HCIClass::tryResolveAddress(uint8_t* BDAddr, uint8_t* address){ + uint8_t iphone[16] = {0xA6, 0xD2, 0xD, 0xD3, 0x4F, 0x13, 0x42, 0x4F, 0xE1, 0xC1, 0xFD, 0x22, 0x2E, 0xC5, 0x6A, 0x2D}; + uint8_t irk[16]; + for(int i=0; i<16; i++) irk[15-i] = iphone[i]; + bool foundMatch = false; + if(HCI._getIRKs!=0){ + uint8_t nIRKs = 0; + uint8_t** BDAddrType = new uint8_t*; + uint8_t*** BADDRs = new uint8_t**; + uint8_t*** IRKs = new uint8_t**; + uint8_t* memcheck; + + + if(!HCI._getIRKs(&nIRKs, BDAddrType, BADDRs, IRKs)){ + Serial.println("error getting IRKs."); + } + for(int i=0; ienabled,1); #endif if(encryptionChange->enabled>0){ + // 0001 1110 + if((ATT.getPeerEncryption(encryptionChange->connectionHandle)&PEER_ENCRYPTION::PAIRING_REQUEST)>0){ + if(ATT.localKeyDistribution.EncKey()){ +#ifdef _BLE_TRACE_ + Serial.println("Enc key set but sould be ignored"); +#endif + }else{ +#ifdef _BLE_TRACE_ + Serial.println("No enc key distribution"); +#endif + } + // From page 1681 bluetooth standard - order matters + if(ATT.localKeyDistribution.IdKey()){ + /// We shall distribute IRK and address using identity information + { + uint8_t response[17]; + response[0] = CONNECTION_IDENTITY_INFORMATION; // Identity information. + for(int i=0; i<16; i++) response[16-i] = ATT.localIRK[i]; + HCI.sendAclPkt(encryptionChange->connectionHandle, SECURITY_CID, sizeof(response), response); +#ifdef _BLE_TRACE_ + Serial.println("Distribute ID Key"); +#endif + } + { + uint8_t response[8]; + response[0] = CONNECTION_IDENTITY_ADDRESS; // Identity address information + response[1] = 0x00; // Static local address + for(int i=0; i<6; i++) response[7-i] = HCI.localAddr[i]; + HCI.sendAclPkt(encryptionChange->connectionHandle, SECURITY_CID, sizeof(response), response); + } + } + if(ATT.localKeyDistribution.SignKey()){ + /// We shall distribut CSRK +#ifdef _BLE_TRACE_ + Serial.println("We shall distribute CSRK // not implemented"); +#endif + + }else{ + // Serial.println("We don't want to distribute CSRK"); + } + if(ATT.localKeyDistribution.LinkKey()){ +#ifdef _BLE_TRACE_ + Serial.println("We would like to use LTK to generate BR/EDR // not implemented"); +#endif + } + }else{ +#ifdef _BLE_TRACE_ + Serial.println("Reconnection, not pairing so no keys"); + Serial.println(ATT.getPeerEncryption(encryptionChange->connectionHandle),HEX); +#endif + } + ATT.setPeerEncryption(encryptionChange->connectionHandle, PEER_ENCRYPTION::ENCRYPTED_AES); if(ATT.writeBufferSize > 0){ ATT.processWriteBuffer(); @@ -740,6 +948,27 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) data += 2; } } + else if(eventHdr->evt == EVT_RETURN_LINK_KEYS) + { + uint8_t num_keys = (uint8_t)pdata[sizeof(HCIEventHdr)]; + // Serial.print("N keys: "); + // Serial.println(num_keys); + uint8_t BD_ADDRs[num_keys][6]; + uint8_t LKs[num_keys][16]; + auto nAddresss = [pdata](uint8_t nAddr)->uint8_t*{ + return (uint8_t*) &pdata[sizeof(HCIEventHdr)] + 1 + nAddr*6 + nAddr*16; + }; + auto nLK = [pdata](uint8_t nLK)->uint8_t*{ + return (uint8_t*) &pdata[sizeof(HCIEventHdr)] + 1 + (nLK+1)*6 + nLK*16; + }; + // Serial.println("Stored LKs are: "); + // for(int i=0; ievt == 0x10) { struct __attribute__ ((packed)) CmdHardwareError { @@ -761,6 +990,57 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) Serial.println(leMetaHeader->subevent,HEX); #endif switch((LE_META_EVENT)leMetaHeader->subevent){ + case 0x0A:{ + struct __attribute__ ((packed)) EvtLeConnectionComplete { + uint8_t status; + uint16_t handle; + uint8_t role; + uint8_t peerBdaddrType; + uint8_t peerBdaddr[6]; + uint8_t localResolvablePrivateAddress[6]; + uint8_t peerResolvablePrivateAddress[6]; + uint16_t interval; + uint16_t latency; + uint16_t supervisionTimeout; + uint8_t masterClockAccuracy; + } *leConnectionComplete = (EvtLeConnectionComplete*)&pdata[sizeof(HCIEventHdr) + sizeof(LeMetaEventHeader)]; + + if (leConnectionComplete->status == 0x00) { + ATT.addConnection(leConnectionComplete->handle, + leConnectionComplete->role, + leConnectionComplete->peerBdaddrType, + leConnectionComplete->peerBdaddr, + leConnectionComplete->interval, + leConnectionComplete->latency, + leConnectionComplete->supervisionTimeout, + leConnectionComplete->masterClockAccuracy); + + L2CAPSignaling.addConnection(leConnectionComplete->handle, + leConnectionComplete->role, + leConnectionComplete->peerBdaddrType, + leConnectionComplete->peerBdaddr, + leConnectionComplete->interval, + leConnectionComplete->latency, + leConnectionComplete->supervisionTimeout, + leConnectionComplete->masterClockAccuracy); + } + // uint8_t address[6]; + // uint8_t BDAddr[6]; + // for(int i=0; i<6; i++) BDAddr[5-i] = leConnectionComplete->peerBdaddr[i]; + // leReadPeerResolvableAddress(leConnectionComplete->peerBdaddrType,BDAddr,address); + // Serial.print("Resolving address: "); + // btct.printBytes(BDAddr, 6); + // Serial.print("BT answer : "); + // btct.printBytes(address, 6); + +#ifdef _BLE_TRACE_ + Serial.print("Resolved peer : "); + btct.printBytes(leConnectionComplete->peerResolvablePrivateAddress,6); + Serial.print("Resolved local : "); + btct.printBytes(leConnectionComplete->localResolvablePrivateAddress,6); +#endif + break; + } case CONN_COMPLETE:{ struct __attribute__ ((packed)) EvtLeConnectionComplete { uint8_t status; @@ -793,6 +1073,14 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) leConnectionComplete->supervisionTimeout, leConnectionComplete->masterClockAccuracy); } + uint8_t address[6]; + uint8_t BDAddr[6]; + for(int i=0; i<6; i++) BDAddr[5-i] = leConnectionComplete->peerBdaddr[i]; + // leReadPeerResolvableAddress(leConnectionComplete->peerBdaddrType,BDAddr,address); + // Serial.print("Resolving address: "); + // btct.printBytes(BDAddr, 6); + // Serial.print("BT answer : "); + // btct.printBytes(address, 6); break; } case ADVERTISING_REPORT:{ @@ -818,8 +1106,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) } break; } - case LONG_TERM_KEY_REQUEST: - { + case LONG_TERM_KEY_REQUEST:{ struct __attribute__ ((packed)) LTKRequest { uint8_t subEventCode; @@ -836,14 +1123,25 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) Serial.print("EDIV : "); btct.printBytes(ltkRequest->encryptedDiversifier,2); #endif - + // Load our LTK for this connection. + uint8_t peerAddr[7]; + uint8_t resolvableAddr[6]; + ATT.getPeerAddrWithType(ltkRequest->connectionHandle, peerAddr); + + if(ATT.getPeerResolvedAddress(ltkRequest->connectionHandle, resolvableAddr) + && !((ATT.getPeerEncryption(ltkRequest->connectionHandle) & PEER_ENCRYPTION::PAIRING_REQUEST)>0)){ + _getLTK(resolvableAddr, HCI.LTK); + }else{ + _getLTK(&peerAddr[1], HCI.LTK); + } + // } // Send our LTK back struct __attribute__ ((packed)) LTKReply { uint16_t connectionHandle; uint8_t LTK[16]; } ltkReply = {0,0}; - ltkReply.connectionHandle = ATT.getPeerEncrptingConnectionHandle(); + ltkReply.connectionHandle = ltkRequest->connectionHandle; for(int i=0; i<16; i++) ltkReply.LTK[15-i] = HCI.LTK[i]; int result = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::LONG_TERM_KEY_REPLY,sizeof(ltkReply), <kReply); @@ -875,8 +1173,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) } break; } - case REMOTE_CONN_PARAM_REQ: - { + case REMOTE_CONN_PARAM_REQ:{ struct __attribute__ ((packed)) RemoteConnParamReq { uint8_t subEventCode; uint16_t connectionHandle; @@ -914,8 +1211,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) sendCommand(OGF_LE_CTL << 10 | 0x20, sizeof(remoteConnParamReqReply), &remoteConnParamReqReply); break; } - case READ_LOCAL_P256_COMPLETE: - { + case READ_LOCAL_P256_COMPLETE:{ struct __attribute__ ((packed)) EvtReadLocalP256Complete{ uint8_t subEventCode; uint8_t status; @@ -1007,8 +1303,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) } break; } - case GENERATE_DH_KEY_COMPLETE: - { + case GENERATE_DH_KEY_COMPLETE:{ struct __attribute__ ((packed)) EvtLeDHKeyComplete{ uint8_t subEventCode; uint8_t status; diff --git a/src/utility/HCI.h b/src/utility/HCI.h index 0b9c8302..de15bdf8 100644 --- a/src/utility/HCI.h +++ b/src/utility/HCI.h @@ -90,6 +90,17 @@ class HCIClass { virtual int leCancelConn(); virtual int leEncrypt(uint8_t* Key, uint8_t* plaintext, uint8_t* status, uint8_t* ciphertext); + 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); + virtual int leStopResolvingAddresses(); + virtual int leStartResolvingAddresses(); + virtual int leReadPeerResolvableAddress(uint8_t peerAddressType, uint8_t* peerIdentityAddress, uint8_t* peerResolvableAddress); + + virtual int readStoredLKs(); + virtual int readStoredLK(uint8_t BD_ADDR[], uint8_t read_all = 0); + virtual int writeLK(uint8_t peerAddress[], uint8_t LK[]); + virtual int tryResolveAddress(uint8_t* BDAddr, uint8_t* address); + virtual int sendAclPkt(uint16_t handle, uint8_t cid, uint8_t plen, void* data); virtual int disconnect(uint16_t handle); @@ -105,6 +116,12 @@ class HCIClass { uint8_t DHKey[32]; uint8_t localAddr[6]; uint8_t LTK[16]; + + int (*_storeIRK)(uint8_t* address, uint8_t* peerIrk); + int (*_getIRKs)(uint8_t* nIRKs,uint8_t** BADDR_type, uint8_t*** BADDRs, uint8_t*** IRKs); + int (*_storeLTK)(uint8_t*, uint8_t*); + int (*_getLTK)(uint8_t*, uint8_t*); + private: virtual void handleAclDataPkt(uint8_t plen, uint8_t pdata[]); diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index 3779eb29..512e7665 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -21,6 +21,7 @@ #include "ATT.h" #include "btct.h" #include "L2CAPSignaling.h" +#include "keyDistribution.h" #define CONNECTION_PARAMETER_UPDATE_REQUEST 0x12 #define CONNECTION_PARAMETER_UPDATE_RESPONSE 0x13 @@ -137,13 +138,22 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t initiatorKeyDistribution; uint8_t responderKeyDistribution; } *pairingRequest = (PairingRequest*)l2capSignalingHdr->data; + + + ATT.remoteKeyDistribution = KeyDistribution(pairingRequest->initiatorKeyDistribution); + ATT.localKeyDistribution = KeyDistribution(pairingRequest->responderKeyDistribution); + KeyDistribution rkd(pairingRequest->responderKeyDistribution); uint8_t peerIOCap[3]; peerIOCap[0] = pairingRequest->authReq; peerIOCap[1] = pairingRequest->oobDataFlag; peerIOCap[2] = pairingRequest->ioCapability; ATT.setPeerIOCap(connectionHandle, peerIOCap); - + ATT.setPeerEncryption(connectionHandle, ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::PAIRING_REQUEST); +#ifdef _BLE_TRACE_ + Serial.print("Peer encryption : 0b"); + Serial.print(ATT.getPeerEncryption(connectionHandle), BIN); +#endif struct __attribute__ ((packed)) PairingResponse { uint8_t code; uint8_t ioCapability; @@ -154,7 +164,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t responderKeyDistribution; } response = { CONNECTION_PAIRING_RESPONSE, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 0x10, 0b1011, 0b1011}; - HCI.sendAclPkt(connectionHandle, 0x06, sizeof(response), &response); + HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); } else if (code == CONNECTION_PAIRING_RANDOM) { @@ -173,7 +183,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t } response = { CONNECTION_PAIRING_RANDOM, 0}; for(int i=0; i< 16; i++) response.Nb[15-i] = HCI.Nb[i]; - HCI.sendAclPkt(connectionHandle, 0x06, sizeof(response), &response); + HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); } else if (code == CONNECTION_PAIRING_RESPONSE) { @@ -190,6 +200,31 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t Serial.println(pairingFailed->reason,HEX); #endif } + else if (code == CONNECTION_IDENTITY_INFORMATION){ + struct __attribute__ ((packed)) IdentityInformation { + uint8_t code; + uint8_t PeerIRK[16]; + } *identityInformation = (IdentityInformation*)data; + for(int i=0; i<16; i++) ATT.peerIRK[15-i] = identityInformation->PeerIRK[i]; +#ifdef _BLE_TRACE_ + Serial.println("Saved peer IRK"); +#endif + } + else if (code == CONNECTION_IDENTITY_ADDRESS){ + struct __attribute__ ((packed)) IdentityAddress { + uint8_t code; + uint8_t addressType; + uint8_t address[6]; + } *identityAddress = (IdentityAddress*)data; + // we can save this information now. + uint8_t peerAddress[6]; + for(int i; i<6; i++) peerAddress[5-i] = identityAddress->address[i]; + + HCI.saveNewAddress(identityAddress->addressType, peerAddress, ATT.peerIRK, ATT.localIRK); + if(HCI._storeLTK!=0){ + HCI._storeLTK(peerAddress, HCI.LTK); + } + } else if (code == CONNECTION_PAIRING_PUBLIC_KEY){ /// Received a public key struct __attribute__ ((packed)) ConnectionPairingPublicKey { @@ -211,7 +246,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t LE_COMMAND::READ_LOCAL_P256, }; - if(ATT.setPeerEncryption(connectionHandle,PEER_ENCRYPTION::REQUESTED_ENCRYPTION)){ + if(ATT.setPeerEncryption(connectionHandle, ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::REQUESTED_ENCRYPTION)){ #ifdef _BLE_TRACE_ Serial.println("[Info] Pairing public key"); Serial.println("Requested encryption stored."); @@ -244,7 +279,7 @@ 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){ // We've already calculated the DHKey so we can calculate our check and send it. uint8_t MacKey[16]; diff --git a/src/utility/btct.cpp b/src/utility/btct.cpp index 8fad5173..d78e328e 100644 --- a/src/utility/btct.cpp +++ b/src/utility/btct.cpp @@ -119,6 +119,38 @@ int BluetoothCryptoToolbox::f6(uint8_t W[], uint8_t N1[],uint8_t N2[],uint8_t R[ return 1; } // AES_CMAC from RFC +int BluetoothCryptoToolbox::ah(uint8_t k[16], uint8_t r[3], uint8_t* result) +{ + uint8_t r_[16]; + int i=0; + for(i=0; i<16; i++) r_[i] = 0; + for(i=0; i<3; i++) r_[i+13] = r[i]; + uint8_t intermediate[16]; + AES_128(k,r_,intermediate); + for(i=0; i<3; i++){ + result[i] = intermediate[i+13]; + } + return 1; +} +void BluetoothCryptoToolbox::testAh() +{ + uint8_t irk[16] = {0xec,0x02,0x34,0xa3,0x57,0xc8,0xad,0x05,0x34,0x10,0x10,0xa6,0x0a,0x39,0x7d,0x9b}; + uint8_t r[3] = {0x70,0x81,0x94}; + uint8_t expected_AES[16] = {0x15,0x9d,0x5f,0xb7,0x2e,0xbe,0x23,0x11,0xa4,0x8c,0x1b,0xdc,0xc4,0x0d,0xfb,0xaa}; + uint8_t expected_final[3] = {0x0d,0xfb,0xaa}; + + for(int i=0; i<3; i++) r[2-i] = expected_final[3+i]; + uint8_t ourResult[3]; + ah(irk, expected_final, ourResult); + + + Serial.print("Expected : "); + printBytes(&expected_final[3], 3); + Serial.print("Actual : "); + printBytes(ourResult, 3); +} + + 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 87e515af..43c0b0d6 100644 --- a/src/utility/btct.h +++ b/src/utility/btct.h @@ -13,9 +13,11 @@ 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 ah(uint8_t k[16], uint8_t r[3], uint8_t result[3]); void test(); void testF5(); void testF6(); + void testAh(); private: int AES_128(uint8_t key[], uint8_t data_in[], uint8_t data_out[]); void leftshift_onebit(unsigned char *input,unsigned char *output); diff --git a/src/utility/keyDistribution.cpp b/src/utility/keyDistribution.cpp new file mode 100644 index 00000000..1a9fe099 --- /dev/null +++ b/src/utility/keyDistribution.cpp @@ -0,0 +1,24 @@ +#include "keyDistribution.h" + +KeyDistribution::KeyDistribution(){} +KeyDistribution::KeyDistribution(uint8_t octet):_octet(octet){} + +#define ENCKEY 0b00000001 +#define IDKEY 0b00000010 +#define SIGNKEY 0b00000100 +#define LINKKEY 0b00001000 +void KeyDistribution::setOctet( uint8_t octet){_octet = octet;} +uint8_t KeyDistribution::getOctet() {return _octet;} +// Ignored when SMP is on LE transport +bool KeyDistribution::EncKey(){ return (_octet & ENCKEY)>0;} +// Device shall distribute IRK using Identity information command followed by its address using Identity address information +bool KeyDistribution::IdKey(){ return (_octet & IDKEY)>0;} +// Device shall distribute CSRK using signing information command +bool KeyDistribution::SignKey(){ return (_octet & SIGNKEY)>0;} +// Device would like to derive BR/EDR from LTK +bool KeyDistribution::LinkKey(){ return (_octet & LINKKEY)>0;} + +void KeyDistribution::setEncKey(bool state) { _octet= state? _octet|ENCKEY : _octet&~ENCKEY;} +void KeyDistribution::setIdKey(bool state) { _octet= state? _octet|IDKEY : _octet&~IDKEY;} +void KeyDistribution::setSignKey(bool state){ _octet= state? _octet|SIGNKEY : _octet&~SIGNKEY;} +void KeyDistribution::setLinkKey(bool state){ _octet= state? _octet|LINKKEY : _octet&~LINKKEY;} \ No newline at end of file diff --git a/src/utility/keyDistribution.h b/src/utility/keyDistribution.h new file mode 100644 index 00000000..d78fcc1a --- /dev/null +++ b/src/utility/keyDistribution.h @@ -0,0 +1,29 @@ +#ifndef _KEY_DISTRIBUTION_H_ +#define _KEY_DISTRIBUTION_H_ +#include + +class KeyDistribution{ +public: + KeyDistribution(); + KeyDistribution(uint8_t octet); + void setOctet( uint8_t octet); + uint8_t getOctet(); + // Ignored when SMP is on LE transport + bool EncKey(); + // Device shall distribute IRK using Identity information command followed by its address using Identity address information + bool IdKey(); + // Device shall distribute CSRK using signing information command + bool SignKey(); + // Device would like to derive BR/EDR from LTK + bool LinkKey(); + + void setEncKey(bool state); + void setIdKey(bool state); + void setSignKey(bool state); + void setLinkKey(bool state); +private: + uint8_t _octet; + // 1. IRK by the slave2. BD ADDR by the slave3. CSRK by the slave4. IRK by the master5. BD_ADDR by the master6. CSRK by the master +}; + +#endif \ No newline at end of file