diff --git a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino new file mode 100644 index 00000000..9f9d453b --- /dev/null +++ b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino @@ -0,0 +1,265 @@ +/* + 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 + + +#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 + +#define CTRL_LED LED_BUILTIN + + + // 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 +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 + while (!Serial); + + pinMode(CTRL_LED, OUTPUT); // initialize the built-in LED pin to indicate when a central is connected + pinMode(PAIR_LED, OUTPUT); + pinMode(PAIR_BUTTON, INPUT_PULLUP); + + + 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. + 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]; + *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 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 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}; + + + (*BDaddrTypes)[0] = 0; // Type 0 is for pubc address, type 1 is for static random + (*BDAddrs)[0] = new uint8_t[6]; + (*IRKs)[0] = new uint8_t[16]; + memcpy((*IRKs)[0] , device1IRK,16); + memcpy((*BDAddrs)[0], device1Mac, 6); + + + (*BDaddrTypes)[1] = 0; + (*BDAddrs)[1] = new uint8_t[6]; + (*IRKs)[1] = new uint8_t[16]; + memcpy((*IRKs)[1] , device2IRK,16); + memcpy((*BDAddrs)[1], device2Mac, 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("Received request for address: "); + btct.printBytes(address,6); + + // Set these to the MAC and LTK of your devices after bonding. + 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(device1Mac, address, 6) == 0) { + memcpy(LTK, device1LTK, 16); + return 1; + }else if(memcmp(device2Mac, address, 6) == 0) { + memcpy(LTK, device2LTK, 16); + return 1; + } + 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); + + // prevent pairing until button is pressed (will show a pairing rejected message) + BLE.setPairable(false); + + /* 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 button is pressed, allow pairing for 30 sec + if (!BLE.pairable() && digitalRead(PAIR_BUTTON) == LOW){ + pairingStarted = millis(); + BLE.setPairable(Pairable::ONCE); + Serial.println("Accepting pairing for 30s"); + } else if (BLE.pairable() && millis() > pairingStarted + PAIR_INTERVAL){ + BLE.setPairable(false); + Serial.println("No longer accepting pairing"); + } + // Make LED blink while pairing is allowed + digitalWrite(PAIR_LED, (BLE.pairable() ? (millis()%400)<200 : BLE.paired()) ? PAIR_LED_ON : !PAIR_LED_ON); + + + // if a central is connected to the peripheral: + if (central && central.connected()) { + if (!wasConnected){ + wasConnected = true; + 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: + long currentMillis = millis(); + // if 200ms have passed, check the battery level: + if (currentMillis - previousMillis >= 1000) { + previousMillis = currentMillis; + updateBatteryLevel(); + digitalWrite(CTRL_LED, secretValue.value()>0 ? HIGH : LOW); + } + } else if (wasConnected){ + wasConnected = false; + 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 + } +} \ No newline at end of file diff --git a/keywords.txt b/keywords.txt index 464cac72..effdfbca 100644 --- a/keywords.txt +++ b/keywords.txt @@ -77,9 +77,12 @@ setEventHandler KEYWORD2 setAdvertisingInterval KEYWORD2 setConnectionInterval KEYWORD2 setConnectable KEYWORD2 +setPairable KEYWORD2 setTimeout KEYWORD2 debug KEYWORD2 noDebug KEYWORD2 +pairable KEYWORD2 +paired KEYWORD2 properties KEYWORD2 valueSize KEYWORD2 diff --git a/src/ArduinoBLE.h b/src/ArduinoBLE.h index dc6e8188..588d5cb1 100644 --- a/src/ArduinoBLE.h +++ b/src/ArduinoBLE.h @@ -24,5 +24,6 @@ #include "BLEProperty.h" #include "BLEStringCharacteristic.h" #include "BLETypedCharacteristics.h" +#include "utility/btct.h" #endif 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 6cdd888f..434bd2aa 100644 --- a/src/BLEProperty.h +++ b/src/BLEProperty.h @@ -17,6 +17,8 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */ +// #include + #ifndef _BLE_PROPERTY_H_ #define _BLE_PROPERTY_H_ @@ -26,7 +28,52 @@ enum BLEProperty { BLEWriteWithoutResponse = 0x04, BLEWrite = 0x08, BLENotify = 0x10, - BLEIndicate = 0x20 + BLEIndicate = 0x20, + 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 */ +#define ESP_GATT_CHAR_PROP_BIT_WRITE (1 << 3) /* 0x08 */ /* relate to BTA_GATT_CHAR_PROP_BIT_WRITE in bta/bta_gatt_api.h */ +#define ESP_GATT_CHAR_PROP_BIT_NOTIFY (1 << 4) /* 0x10 */ /* relate to BTA_GATT_CHAR_PROP_BIT_NOTIFY in bta/bta_gatt_api.h */ +#define ESP_GATT_CHAR_PROP_BIT_INDICATE (1 << 5) /* 0x20 */ /* relate to BTA_GATT_CHAR_PROP_BIT_INDICATE in bta/bta_gatt_api.h */ +#define ESP_GATT_CHAR_PROP_BIT_AUTH (1 << 6) /* 0x40 */ /* relate to BTA_GATT_CHAR_PROP_BIT_AUTH in bta/bta_gatt_api.h */ +#define ESP_GATT_CHAR_PROP_BIT_EXT_PROP (1 << 7) /* 0x80 */ /* relate to BTA_GATT_CHAR_PROP_BIT_EXT_PROP in bta/bta_gatt_api.h */ + +#define ESP_GATT_PERM_READ (1 << 0) /* bit 0 - 0x0001 */ /* relate to BTA_GATT_PERM_READ in bta/bta_gatt_api.h */ +#define ESP_GATT_PERM_READ_ENCRYPTED (1 << 1) /* bit 1 - 0x0002 */ /* relate to BTA_GATT_PERM_READ_ENCRYPTED in bta/bta_gatt_api.h */ +#define ESP_GATT_PERM_READ_ENC_MITM (1 << 2) /* bit 2 - 0x0004 */ /* relate to BTA_GATT_PERM_READ_ENC_MITM in bta/bta_gatt_api.h */ +#define ESP_GATT_PERM_WRITE (1 << 4) /* bit 4 - 0x0010 */ /* relate to BTA_GATT_PERM_WRITE in bta/bta_gatt_api.h */ +#define ESP_GATT_PERM_WRITE_ENCRYPTED (1 << 5) /* bit 5 - 0x0020 */ /* relate to BTA_GATT_PERM_WRITE_ENCRYPTED in bta/bta_gatt_api.h */ +#define ESP_GATT_PERM_WRITE_ENC_MITM (1 << 6) /* bit 6 - 0x0040 */ /* relate to BTA_GATT_PERM_WRITE_ENC_MITM in bta/bta_gatt_api.h */ +#define ESP_GATT_PERM_WRITE_SIGNED (1 << 7) /* bit 7 - 0x0080 */ /* relate to BTA_GATT_PERM_WRITE_SIGNED in bta/bta_gatt_api.h */ +#define ESP_GATT_PERM_WRITE_SIGNED_MITM (1 << 8) /* bit 8 - 0x0100 */ /* relate to BTA_GATT_PERM_WRITE_SIGNED_MITM in bta/bta_gatt_api.h */ +#define ESP_GATT_PERM_READ_AUTHORIZATION (1 << 9) /* bit 9 - 0x0200 */ +#define ESP_GATT_PERM_WRITE_AUTHORIZATION (1 << 10) /* bit 10 - 0x0400 */ + +enum BLE_GATT_PERM_ { + READ = 1 << 0, + READ_ENCRYPTED = 1 << 1, + READ_ENC_MITM = 1 << 2, + WRITE = 1 << 4, + WRITE_ENCRYPTED = 1 << 5, + WRITE_ENC_MITM = 1 << 6, + WRITE_SIGNED = 1 << 7, + WRITE_SIGNED_MITM = 1 << 8, + READ_AUTHORIZATION = 1 << 9, + WRITE_AUTHORIZATION = 1 << 10, }; + #endif 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 0ca0220f..8c1758b0 100644 --- a/src/local/BLELocalDevice.cpp +++ b/src/local/BLELocalDevice.cpp @@ -108,6 +108,10 @@ int BLELocalDevice::begin() end(); return 0; } + if (HCI.setLeEventMask(0x00000000000003FF) != 0) { + end(); + return 0; + } uint16_t pktLen; uint8_t maxPkt; @@ -117,6 +121,59 @@ int BLELocalDevice::begin() return 0; } + /// The HCI should allow automatic address resolution. + + // // If we have callbacks to remember 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 a1028f76..f6ab392f 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), @@ -95,6 +97,7 @@ ATTClass::ATTClass() : memset(_peers[i].address, 0x00, sizeof(_peers[i].address)); _peers[i].mtu = 23; _peers[i].device = NULL; + _peers[i].encryption = 0x0; } memset(_eventHandlers, 0x00, sizeof(_eventHandlers)); @@ -252,6 +255,18 @@ 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{ +#ifdef _BLE_TRACE_ + Serial.println("No matching MAC"); +#endif + memset(&_peers[peerIndex].resolvedAddress, 0, 6); + } if (_eventHandlers[BLEConnected]) { _eventHandlers[BLEConnected](BLEDevice(peerBdaddrType, peerBdaddr)); @@ -267,12 +282,24 @@ void ATTClass::handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[ uint16_t mtu = this->mtu(connectionHandle); +#ifdef _BLE_TRACE_ + Serial.print("data opcode: 0x"); + Serial.println(opcode, HEX); +#endif switch (opcode) { case ATT_OP_ERROR: +#ifdef _BLE_TRACE_ + Serial.println("[Info] data error"); + // Serial.print("Error: "); + // btct.printBytes(data, dlen); +#endif error(connectionHandle, dlen, data); break; case ATT_OP_MTU_REQ: +#ifdef _BLE_TRACE_ + Serial.println("MTU"); +#endif mtuReq(connectionHandle, dlen, data); break; @@ -281,6 +308,9 @@ void ATTClass::handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[ break; case ATT_OP_FIND_INFO_REQ: +#ifdef _BLE_TRACE_ + Serial.println("Find info"); +#endif findInfoReq(connectionHandle, mtu, dlen, data); break; @@ -293,6 +323,9 @@ void ATTClass::handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[ break; case ATT_OP_READ_BY_TYPE_REQ: +#ifdef _BLE_TRACE_ + Serial.println("By type"); +#endif readByTypeReq(connectionHandle, mtu, dlen, data); break; @@ -319,6 +352,9 @@ void ATTClass::handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[ case ATT_OP_WRITE_REQ: case ATT_OP_WRITE_CMD: +#ifdef _BLE_TRACE_ + Serial.println("Write req"); +#endif writeReqOrCmd(connectionHandle, mtu, opcode, dlen, data); break; @@ -346,6 +382,9 @@ void ATTClass::handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[ case ATT_OP_READ_MULTI_REQ: case ATT_OP_SIGNED_WRITE_CMD: default: +#ifdef _BLE_TRACE_ + Serial.println("[Info] Unhandled dara"); +#endif sendError(connectionHandle, opcode, 0x00, ATT_ECODE_REQ_NOT_SUPP); break; } @@ -398,6 +437,10 @@ void ATTClass::removeConnection(uint16_t handle, uint8_t /*reason*/) _peers[peerIndex].addressType = 0x00; memset(_peers[peerIndex].address, 0x00, sizeof(_peers[peerIndex].address)); _peers[peerIndex].mtu = 23; + _peers[peerIndex].encryption = PEER_ENCRYPTION::NO_ENCRYPTION; + _peers[peerIndex].IOCap[0] = 0; + _peers[peerIndex].IOCap[1] = 0; + _peers[peerIndex].IOCap[2] = 0; if (_peers[peerIndex].device) { delete _peers[peerIndex].device; @@ -456,6 +499,32 @@ bool ATTClass::connected(uint16_t handle) const return false; } +/* + * Return true if any of the known devices is paired (peer encrypted) + * Does not check if the paired device is also connected + */ +bool ATTClass::paired() const +{ + for(int i=0; i 0){ + return true; + } + } + return false; +} + +/* + * Return true if the specified device is paired (peer encrypted) + */ +bool ATTClass::paired(uint16_t handle) const +{ + for(int i=0; i 0; + } + return false; // unknown handle +} + uint16_t ATTClass::mtu(uint16_t handle) const { for (int i = 0; i < ATT_MAX_PEERS; i++) { @@ -486,6 +555,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) { @@ -532,6 +602,7 @@ bool ATTClass::handleNotify(uint16_t handle, const uint8_t* value, int length) memcpy(¬ification[notificationLength], value, length); notificationLength += length; + /// TODO: Set encryption requirement on notify. HCI.sendAclPkt(_peers[i].connectionHandle, ATT_CID, notificationLength, notification); numNotifications++; @@ -808,6 +879,14 @@ void ATTClass::readByGroupReq(uint16_t connectionHandle, uint16_t mtu, uint8_t d uint16_t endHandle; uint16_t uuid; } *readByGroupReq = (ReadByGroupReq*)data; +#ifdef _BLE_TRACE_ + Serial.print("readByGroupReq: start: 0x"); + Serial.println(readByGroupReq->startHandle,HEX); + Serial.print("readByGroupReq: end: 0x"); + Serial.println(readByGroupReq->endHandle,HEX); + Serial.print("readByGroupReq: UUID: 0x"); + Serial.println(readByGroupReq->uuid,HEX); +#endif if (dlen != sizeof(ReadByGroupReq) || (readByGroupReq->uuid != BLETypeService && readByGroupReq->uuid != 0x2801)) { sendError(connectionHandle, ATT_OP_READ_BY_GROUP_REQ, readByGroupReq->startHandle, ATT_ECODE_UNSUPP_GRP_TYPE); @@ -820,7 +899,10 @@ void ATTClass::readByGroupReq(uint16_t connectionHandle, uint16_t mtu, uint8_t d response[0] = ATT_OP_READ_BY_GROUP_RESP; response[1] = 0x00; responseLength = 2; - +#ifdef _BLE_TRACE_ + Serial.print("readByGroupReq: attrcount: "); + Serial.println(GATT.attributeCount()); +#endif for (uint16_t i = (readByGroupReq->startHandle - 1); i < GATT.attributeCount() && i <= (readByGroupReq->endHandle - 1); i++) { BLELocalAttribute* attribute = GATT.attribute(i); @@ -907,6 +989,8 @@ void ATTClass::readOrReadBlobReq(uint16_t connectionHandle, uint16_t mtu, uint8_ return; } } + /// if auth error, hold the response in a buffer. + bool holdResponse = false; uint16_t handle = *(uint16_t*)data; uint16_t offset = (opcode == ATT_OP_READ_REQ) ? 0 : *(uint16_t*)&data[sizeof(handle)]; @@ -963,6 +1047,12 @@ void ATTClass::readOrReadBlobReq(uint16_t connectionHandle, uint16_t mtu, uint8_ sendError(connectionHandle, opcode, handle, ATT_ECODE_READ_NOT_PERM); return; } + // If characteristic requires encryption send error & hold response until encrypted + if ((characteristic->permissions() & (BLEPermission::BLEEncryption >> 8)) > 0 && + (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES)==0 ) { + holdResponse = true; + sendError(connectionHandle, opcode, handle, ATT_ECODE_INSUFF_ENC); + } uint16_t valueLength = characteristic->valueLength(); @@ -995,8 +1085,12 @@ void ATTClass::readOrReadBlobReq(uint16_t connectionHandle, uint16_t mtu, uint8_ memcpy(&response[responseLength], descriptor->value() + offset, valueLength); responseLength += valueLength; } - - HCI.sendAclPkt(connectionHandle, ATT_CID, responseLength, response); + if(holdResponse){ + memcpy(holdBuffer, response, responseLength); + holdBufferSize = responseLength; + }else{ + HCI.sendAclPkt(connectionHandle, ATT_CID, responseLength, response); + } } void ATTClass::readResp(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]) @@ -1165,6 +1259,7 @@ void ATTClass::writeReqOrCmd(uint16_t connectionHandle, uint16_t mtu, uint8_t op uint8_t* value = &data[sizeof(handle)]; BLELocalAttribute* attribute = GATT.attribute(handle - 1); + bool holdResponse = false; if (attribute->type() == BLETypeCharacteristic) { BLELocalCharacteristic* characteristic = (BLELocalCharacteristic*)attribute; @@ -1177,10 +1272,34 @@ void ATTClass::writeReqOrCmd(uint16_t connectionHandle, uint16_t mtu, uint8_t op } return; } + // Check permssion + 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); + } for (int i = 0; i < ATT_MAX_PEERS; i++) { if (_peers[i].connectionHandle == connectionHandle) { - characteristic->writeValue(BLEDevice(_peers[i].addressType, _peers[i].address), value, valueLength); + if(holdResponse){ + + writeBufferSize = 0; + memcpy(writeBuffer, &handle, 2); + writeBufferSize+=2; + + writeBuffer[writeBufferSize++] = _peers[i].addressType; + + memcpy(&writeBuffer[writeBufferSize], _peers[i].address, sizeof(_peers[i].address)); + writeBufferSize += sizeof(_peers[i].address); + + writeBuffer[writeBufferSize] = valueLength; + writeBufferSize += sizeof(valueLength); + + memcpy(&writeBuffer[writeBufferSize], value, valueLength); + writeBufferSize += valueLength; + }else{ + characteristic->writeValue(BLEDevice(_peers[i].addressType, _peers[i].address), value, valueLength); + } break; } } @@ -1227,8 +1346,36 @@ void ATTClass::writeReqOrCmd(uint16_t connectionHandle, uint16_t mtu, uint8_t op response[0] = ATT_OP_WRITE_RESP; responseLength = 1; - HCI.sendAclPkt(connectionHandle, ATT_CID, responseLength, response); + if(holdResponse){ + memcpy(holdBuffer, response, responseLength); + holdBufferSize = responseLength; + }else{ + HCI.sendAclPkt(connectionHandle, ATT_CID, responseLength, response); + } + } +} +int ATTClass::processWriteBuffer(){ + if(writeBufferSize==0){ + return 0; } + + struct __attribute__ ((packed)) WriteBuffer { + uint16_t handle; + uint8_t addressType; + uint8_t address[6]; + uint8_t valueLength; + uint8_t value[]; + } *writeBufferStruct = (WriteBuffer*)&ATT.writeBuffer; + // uint8_t value[writeBufferStruct->valueLength]; + // memcpy(value, writeBufferStruct->value, writeBufferStruct->valueLength); + BLELocalAttribute* attribute = GATT.attribute(writeBufferStruct->handle-1); + BLELocalCharacteristic* characteristic = (BLELocalCharacteristic*)attribute; +#ifdef _BLE_TRACE_ + Serial.println("Writing value"); +#endif + characteristic->writeValue(BLEDevice(writeBufferStruct->addressType, writeBufferStruct->address), writeBufferStruct->value, writeBufferStruct->valueLength); + writeBufferSize = 0; + return 1; } void ATTClass::writeResp(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]) @@ -1688,7 +1835,110 @@ void ATTClass::writeCmd(uint16_t connectionHandle, uint16_t handle, const uint8_ sendReq(connectionHandle, &writeReq, 3 + dataLen, NULL); } +// Set encryption state for a peer +int ATTClass::setPeerEncryption(uint16_t connectionHandle, uint8_t encryption){ + for(int i=0; i 0){ + return _peers[i].connectionHandle; + } + } + return ATT_MAX_PEERS + 1; +} +// Get the encryption state for a particular peer / connection handle +uint8_t ATTClass::getPeerEncryption(uint16_t connectionHandle) { + for(int i=0; i #include "BLEDevice.h" +#include "keyDistribution.h" #define ATT_CID 0x0004 +#define BLE_CTL 0x0008 #if DM_CONN_MAX #define ATT_MAX_PEERS DM_CONN_MAX // Mbed + Cordio @@ -34,6 +36,17 @@ #define ATT_MAX_PEERS 8 #endif +enum PEER_ENCRYPTION { + NO_ENCRYPTION = 0, + 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 +}; + class BLERemoteDevice; class ATTClass { @@ -62,6 +75,8 @@ class ATTClass { virtual bool connected() const; virtual bool connected(uint8_t addressType, const uint8_t address[6]) const; virtual bool connected(uint16_t handle) const; + virtual bool paired() const; + virtual bool paired(uint16_t handle) const; virtual uint16_t mtu(uint16_t handle) const; virtual bool disconnect(); @@ -76,7 +91,24 @@ class ATTClass { virtual int readReq(uint16_t connectionHandle, uint16_t handle, uint8_t responseBuffer[]); virtual int writeReq(uint16_t connectionHandle, uint16_t handle, const uint8_t* data, uint8_t dataLen, uint8_t responseBuffer[]); virtual void writeCmd(uint16_t connectionHandle, uint16_t handle, const uint8_t* data, uint8_t dataLen); - + virtual int setPeerEncryption(uint16_t connectionHandle, uint8_t encryption); + uint8_t getPeerEncryption(uint16_t connectionHandle); + uint16_t getPeerEncrptingConnectionHandle(); + virtual int getPeerAddr(uint16_t connectionHandle, uint8_t peerAddr[]); + 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[]); @@ -117,8 +149,11 @@ class ATTClass { uint8_t role; uint8_t addressType; uint8_t address[6]; + uint8_t resolvedAddress[6]; uint16_t mtu; BLERemoteDevice* device; + uint8_t encryption; + uint8_t IOCap[3]; } _peers[ATT_MAX_PEERS]; volatile bool _cnf; diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 233dd1a5..96948b85 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -21,27 +21,29 @@ #include "GAP.h" #include "HCITransport.h" #include "L2CAPSignaling.h" - +#include "btct.h" #include "HCI.h" +#include "bitDescriptions.h" +// #define _BLE_TRACE_ + -#define HCI_COMMAND_PKT 0x01 -#define HCI_ACLDATA_PKT 0x02 -#define HCI_EVENT_PKT 0x04 +#define HCI_COMMAND_PKT 0x01 +#define HCI_ACLDATA_PKT 0x02 +#define HCI_EVENT_PKT 0x04 +#define HCI_SECURITY_PKT 0x06 -#define EVT_DISCONN_COMPLETE 0x05 -#define EVT_CMD_COMPLETE 0xe -#define EVT_CMD_STATUS 0x0f -#define EVT_NUM_COMP_PKTS 0x13 -#define EVT_LE_META_EVENT 0x3e +#define EVT_DISCONN_COMPLETE 0x05 +#define EVT_ENCRYPTION_CHANGE 0x08 +#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 #define EVT_LE_CONN_COMPLETE 0x01 #define EVT_LE_ADVERTISING_REPORT 0x02 -#define OGF_LINK_CTL 0x01 -#define OGF_HOST_CTL 0x03 -#define OGF_INFO_PARAM 0x04 -#define OGF_STATUS_PARAM 0x05 -#define OGF_LE_CTL 0x08 // OGF_LINK_CTL #define OCF_DISCONNECT 0x0006 @@ -72,6 +74,29 @@ #define HCI_OE_USER_ENDED_CONNECTION 0x13 +String metaEventToString(LE_META_EVENT event) +{ + switch(event){ + case CONN_COMPLETE: return F("CONN_COMPLETE"); + case ADVERTISING_REPORT: return F("ADVERTISING_REPORT"); + case LONG_TERM_KEY_REQUEST: return F("LE_LONG_TERM_KEY_REQUEST"); + case READ_LOCAL_P256_COMPLETE: return F("READ_LOCAL_P256_COMPLETE"); + case GENERATE_DH_KEY_COMPLETE: return F("GENERATE_DH_KEY_COMPLETE"); + default: return "event unknown"; + } +} +String commandToString(LE_COMMAND command){ + switch (command) + { + case ENCRYPT: return F("ENCRYPT"); + case LONG_TERM_KEY_REPLY: return F("LONG_TERM_KEY_REPLY"); + case READ_LOCAL_P256: return F("READ_LOCAL_P256"); + case GENERATE_DH_KEY_V1: return F("GENERATE_DH_KEY_V1"); + case GENERATE_DH_KEY_V2: return F("GENERATE_DH_KEY_V2"); + default: return "UNKNOWN"; + } +} + HCIClass::HCIClass() : _debug(NULL), _recvIndex(0), @@ -203,6 +228,17 @@ int HCIClass::readBdAddr(uint8_t addr[6]) return result; } +int HCIClass::readBdAddr(){ + uint8_t response[6]; + int result = readBdAddr(response); + if(result==0){ + for(int i=0; i<6; i++){ + localAddr[5-i] = _cmdResponse[i]; + } + } + return result; +} + int HCIClass::readRssi(uint16_t handle) { int result = sendCommand(OGF_STATUS_PARAM << 10 | OCF_READ_RSSI, sizeof(handle), &handle); @@ -226,6 +262,11 @@ int HCIClass::setEventMask(uint64_t eventMask) { return sendCommand(OGF_HOST_CTL << 10 | OCF_SET_EVENT_MASK, sizeof(eventMask), &eventMask); } +// Set LE Event mask +int HCIClass::setLeEventMask(uint64_t leEventMask) +{ + return sendCommand(OGF_LE_CTL << 10 | 0x01, sizeof(leEventMask), &leEventMask); +} int HCIClass::readLeBufferSize(uint16_t& pktLen, uint8_t& maxPkt) { @@ -412,6 +453,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; i ", sizeof(aclHdr) + plen, txBuffer); } +#ifdef _BLE_TRACE_ + Serial.print("Data tx -> "); + for(int i=0; i< sizeof(aclHdr) + plen;i++){ + Serial.print(" 0x"); + Serial.print(txBuffer[i],HEX); + } + Serial.println("."); +#endif _pendingPkt++; HCITransport.write(txBuffer, sizeof(aclHdr) + plen); @@ -476,7 +680,15 @@ int HCIClass::sendCommand(uint16_t opcode, uint8_t plen, void* parameters) if (_debug) { dumpPkt("HCI COMMAND TX -> ", sizeof(pktHdr) + plen, txBuffer); } - +#ifdef _BLE_TRACE_ + Serial.print("Command tx -> "); + for(int i=0; i< sizeof(pktHdr) + plen;i++){ + Serial.print(" 0x"); + Serial.print(txBuffer[i],HEX); + + } + Serial.println(""); +#endif HCITransport.write(txBuffer, sizeof(pktHdr) + plen); _cmdCompleteOpcode = 0xffff; @@ -498,6 +710,7 @@ void HCIClass::handleAclDataPkt(uint8_t /*plen*/, uint8_t pdata[]) uint16_t cid; } *aclHdr = (HCIACLHdr*)pdata; + uint16_t aclFlags = (aclHdr->handle & 0xf000) >> 12; if ((aclHdr->dlen - 4) != aclHdr->len) { @@ -517,6 +730,17 @@ void HCIClass::handleAclDataPkt(uint8_t /*plen*/, uint8_t pdata[]) } if ((aclHdr->dlen - 4) != aclHdr->len) { +#ifdef _BLE_TRACE_ + Serial.println("Don't have full packet yet"); + Serial.print("Handle: "); + btct.printBytes((uint8_t*)&aclHdr->handle,2); + Serial.print("dlen: "); + btct.printBytes((uint8_t*)&aclHdr->dlen,2); + Serial.print("len: "); + btct.printBytes((uint8_t*)&aclHdr->len,2); + Serial.print("cid: "); + btct.printBytes((uint8_t*)&aclHdr->cid,2); +#endif // don't have the full packet yet return; } @@ -530,8 +754,22 @@ void HCIClass::handleAclDataPkt(uint8_t /*plen*/, uint8_t pdata[]) ATT.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]); } } else if (aclHdr->cid == SIGNALING_CID) { +#ifdef _BLE_TRACE_ + Serial.println("Signalling"); +#endif L2CAPSignaling.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]); - } else { + } else if (aclHdr->cid == SECURITY_CID){ + // Security manager +#ifdef _BLE_TRACE_ + Serial.println("Security data"); +#endif + if (aclFlags == 0x1){ + L2CAPSignaling.handleSecurityData(aclHdr->handle & 0x0fff, aclHdr->len, &_aclPktBuffer[sizeof(HCIACLHdr)]); + }else{ + L2CAPSignaling.handleSecurityData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]); + } + + }else { struct __attribute__ ((packed)) { uint8_t op; uint8_t id; @@ -540,6 +778,10 @@ void HCIClass::handleAclDataPkt(uint8_t /*plen*/, uint8_t pdata[]) uint16_t localCid; uint16_t remoteCid; } l2capRejectCid= { 0x01, 0x00, 0x006, 0x0002, aclHdr->cid, 0x0000 }; +#ifdef _BLE_TRACE_ + Serial.print("rejecting packet cid: 0x"); + Serial.println(aclHdr->cid,HEX); +#endif sendAclPkt(aclHdr->handle & 0x0fff, 0x0005, sizeof(l2capRejectCid), &l2capRejectCid); } @@ -560,8 +802,13 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) uint8_t evt; uint8_t plen; } *eventHdr = (HCIEventHdr*)pdata; +#ifdef _BLE_TRACE_ + Serial.print("HCI event: "); + Serial.println(eventHdr->evt, HEX); +#endif - if (eventHdr->evt == EVT_DISCONN_COMPLETE) { + if (eventHdr->evt == EVT_DISCONN_COMPLETE) + { struct __attribute__ ((packed)) DisconnComplete { uint8_t status; uint16_t handle; @@ -572,98 +819,668 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) L2CAPSignaling.removeConnection(disconnComplete->handle, disconnComplete->reason); HCI.leSetAdvertiseEnable(0x01); - } else if (eventHdr->evt == EVT_CMD_COMPLETE) { + } + else if (eventHdr->evt == EVT_ENCRYPTION_CHANGE) + { + struct __attribute__ ((packed)) EncryptionChange { + uint8_t status; + uint16_t connectionHandle; + uint8_t enabled; + } *encryptionChange = (EncryptionChange*)&pdata[sizeof(HCIEventHdr)]; +#ifdef _BLE_TRACE_ + Serial.println("[Info] Encryption changed"); + Serial.print("status : "); + btct.printBytes(&encryptionChange->status,1); + Serial.print("handle : "); + btct.printBytes((uint8_t*)&encryptionChange->connectionHandle,2); + Serial.print("enabled: "); + btct.printBytes(&encryptionChange->enabled,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 should 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(); + } + if(ATT.holdBufferSize>0){ +#ifdef _BLE_TRACE_ + Serial.print("Sending queued response size: "); + Serial.println(ATT.holdBufferSize); +#endif + HCI.sendAclPkt(encryptionChange->connectionHandle, ATT_CID, ATT.holdBufferSize, ATT.holdBuffer); + ATT.holdBufferSize = 0; + } + }else{ + ATT.setPeerEncryption(encryptionChange->connectionHandle, PEER_ENCRYPTION::NO_ENCRYPTION); + } + } + else if (eventHdr->evt == EVT_CMD_COMPLETE) + { struct __attribute__ ((packed)) CmdComplete { uint8_t ncmd; uint16_t opcode; uint8_t status; } *cmdCompleteHeader = (CmdComplete*)&pdata[sizeof(HCIEventHdr)]; - +#ifdef _BLE_TRACE_ + Serial.print("E ncmd: 0x"); + Serial.println(cmdCompleteHeader->ncmd,HEX); + Serial.print("E opcode: 0x"); + Serial.println(cmdCompleteHeader->opcode, HEX); + Serial.print("E status: 0x"); + Serial.println(cmdCompleteHeader->status, HEX); +#endif _cmdCompleteOpcode = cmdCompleteHeader->opcode; _cmdCompleteStatus = cmdCompleteHeader->status; _cmdResponseLen = pdata[1] - sizeof(CmdComplete); _cmdResponse = &pdata[sizeof(HCIEventHdr) + sizeof(CmdComplete)]; - } else if (eventHdr->evt == EVT_CMD_STATUS) { + } + else if (eventHdr->evt == EVT_CMD_STATUS) + { struct __attribute__ ((packed)) CmdStatus { uint8_t status; uint8_t ncmd; uint16_t opcode; } *cmdStatusHeader = (CmdStatus*)&pdata[sizeof(HCIEventHdr)]; +#ifdef _BLE_TRACE_ + Serial.print("F n cmd: 0x"); + Serial.println(cmdStatusHeader->ncmd, HEX); + Serial.print("F status: 0x"); + Serial.println(cmdStatusHeader->status, HEX); + Serial.print("F opcode: 0x"); + Serial.println(cmdStatusHeader->opcode, HEX); +#endif _cmdCompleteOpcode = cmdStatusHeader->opcode; _cmdCompleteStatus = cmdStatusHeader->status; _cmdResponseLen = 0; - } else if (eventHdr->evt == EVT_NUM_COMP_PKTS) { + } + else if (eventHdr->evt == EVT_NUM_COMP_PKTS) + { uint8_t numHandles = pdata[sizeof(HCIEventHdr)]; uint16_t* data = (uint16_t*)&pdata[sizeof(HCIEventHdr) + sizeof(numHandles)]; for (uint8_t i = 0; i < numHandles; i++) { handleNumCompPkts(data[0], data[1]); - +#ifdef _BLE_TRACE_ + Serial.print("Outstanding packets: "); + Serial.println(_pendingPkt); + Serial.print("Data[0]: 0x"); + Serial.println(data[0]); + Serial.print("Data[1]: 0x"); + Serial.println(data[1]); +#endif data += 2; } - } else if (eventHdr->evt == EVT_LE_META_EVENT) { + } + 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 { + uint8_t hardwareCode; + } *cmdHardwareError = (CmdHardwareError*)&pdata[sizeof(HCIEventHdr)]; +#ifdef _BLE_TRACE_ + Serial.print("Bluetooth hardware error."); + Serial.print(" Code: 0x"); + Serial.println(cmdHardwareError->hardwareCode, HEX); +#endif + } + else if (eventHdr->evt == EVT_LE_META_EVENT) + { struct __attribute__ ((packed)) LeMetaEventHeader { uint8_t subevent; } *leMetaHeader = (LeMetaEventHeader*)&pdata[sizeof(HCIEventHdr)]; +#ifdef _BLE_TRACE_ + Serial.print("\tSubEvent: 0x"); + 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; + uint16_t handle; + uint8_t role; + uint8_t peerBdaddrType; + uint8_t peerBdaddr[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); + break; + } + case ADVERTISING_REPORT:{ + struct __attribute__ ((packed)) EvtLeAdvertisingReport { + uint8_t status; + uint8_t type; + uint8_t peerBdaddrType; + uint8_t peerBdaddr[6]; + uint8_t eirLength; + uint8_t eirData[31]; + } *leAdvertisingReport = (EvtLeAdvertisingReport*)&pdata[sizeof(HCIEventHdr) + sizeof(LeMetaEventHeader)]; + + if (leAdvertisingReport->status == 0x01) { + // last byte is RSSI + int8_t rssi = leAdvertisingReport->eirData[leAdvertisingReport->eirLength]; + + GAP.handleLeAdvertisingReport(leAdvertisingReport->type, + leAdvertisingReport->peerBdaddrType, + leAdvertisingReport->peerBdaddr, + leAdvertisingReport->eirLength, + leAdvertisingReport->eirData, + rssi); + } + break; + } + case LONG_TERM_KEY_REQUEST:{ + struct __attribute__ ((packed)) LTKRequest + { + uint8_t subEventCode; + uint16_t connectionHandle; + uint8_t randomNumber[8]; + uint8_t encryptedDiversifier[2]; + } *ltkRequest = (LTKRequest*)&pdata[sizeof(HCIEventHdr)]; +#ifdef _BLE_TRACE_ + Serial.println("LTK request received"); + Serial.print("Connection Handle: "); + btct.printBytes((uint8_t*)<kRequest->connectionHandle,2); + Serial.print("Random Number : "); + btct.printBytes(ltkRequest->randomNumber,8); + Serial.print("EDIV : "); + btct.printBytes(ltkRequest->encryptedDiversifier,2); +#endif + // Load our LTK for this connection. + uint8_t peerAddr[7]; + uint8_t resolvableAddr[6]; + uint8_t foundLTK; + ATT.getPeerAddrWithType(ltkRequest->connectionHandle, peerAddr); + + if((ATT.getPeerEncryption(ltkRequest->connectionHandle) & PEER_ENCRYPTION::PAIRING_REQUEST)>0){ + // Pairing request - LTK is one in buffer already + foundLTK = 1; + }else{ + if(ATT.getPeerResolvedAddress(ltkRequest->connectionHandle, resolvableAddr)){ + foundLTK = getLTK(resolvableAddr, HCI.LTK); + }else{ + foundLTK = getLTK(&peerAddr[1], HCI.LTK); + } + } + // } //2d + // Send our LTK back + if(foundLTK){ + struct __attribute__ ((packed)) LTKReply + { + uint16_t connectionHandle; + uint8_t LTK[16]; + } ltkReply = {0,0}; + 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); + + #ifdef _BLE_TRACE_ + Serial.println("Sending LTK as: "); + btct.printBytes(ltkReply.LTK,16); + #endif + + if(result == 0){ + struct __attribute__ ((packed)) LTKReplyResult + { + uint8_t status; + uint16_t connectionHandle; + } ltkReplyResult = {0,0}; + memcpy(<kReplyResult, _cmdResponse, 3); + + #ifdef _BLE_TRACE_ + Serial.println("LTK send success"); + Serial.print("status : "); + btct.printBytes(<kReplyResult.status,1); + Serial.print("Conn Handle: "); + btct.printBytes((uint8_t*)<kReplyResult.connectionHandle,2); + #endif + }else{ + #ifdef _BLE_TRACE_ + Serial.print("Failed to send LTK...: "); + btct.printBytes((uint8_t*)&result,2); + #endif + } + }else{ + /// do LTK rejection +#ifdef _BLE_TRACE_ + Serial.println("LTK not found, rejecting"); +#endif + sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::LONG_TERM_KEY_NEGATIVE_REPLY,2, <kRequest->connectionHandle); + } + break; + } + case REMOTE_CONN_PARAM_REQ:{ + struct __attribute__ ((packed)) RemoteConnParamReq { + uint8_t subEventCode; + uint16_t connectionHandle; + uint16_t intervalMin; + uint16_t intervalMax; + uint16_t latency; + uint16_t timeOut; + } *remoteConnParamReq = (RemoteConnParamReq*)&pdata[sizeof(HCIEventHdr)]; +#ifdef _BLE_TRACE_ + Serial.println("--- Remtoe conn param req"); + Serial.print("Handle : "); + btct.printBytes((uint8_t*)&remoteConnParamReq->connectionHandle,2); + Serial.print("Interval min: "); + btct.printBytes((uint8_t*)&remoteConnParamReq->intervalMin,2); + Serial.print("Interval max: "); + btct.printBytes((uint8_t*)&remoteConnParamReq->intervalMax,2); + Serial.print("Latency : "); + btct.printBytes((uint8_t*)&remoteConnParamReq->latency,2); + Serial.print("Timeout : "); + btct.printBytes((uint8_t*)&remoteConnParamReq->timeOut,2); +#endif + + struct __attribute__ ((packed)) RemoteConnParamReqReply { + uint16_t connectionHandle; + uint16_t intervalMin; + uint16_t intervalMax; + uint16_t latency; + uint16_t timeOut; + uint16_t minLength; + uint16_t maxLength; + } remoteConnParamReqReply; + memcpy(&remoteConnParamReqReply, &remoteConnParamReq->connectionHandle, sizeof(RemoteConnParamReq)-1); + + remoteConnParamReqReply.minLength = 0x000F; + remoteConnParamReqReply.maxLength = 0x0FFF; + sendCommand(OGF_LE_CTL << 10 | 0x20, sizeof(RemoteConnParamReqReply), &remoteConnParamReqReply); + break; + } + case READ_LOCAL_P256_COMPLETE:{ + struct __attribute__ ((packed)) EvtReadLocalP256Complete{ + uint8_t subEventCode; + uint8_t status; + uint8_t localPublicKey[64]; + } *evtReadLocalP256Complete = (EvtReadLocalP256Complete*)&pdata[sizeof(HCIEventHdr)]; + if(evtReadLocalP256Complete->status == 0x0){ +#ifdef _BLE_TRACE_ + Serial.println("Key read success"); +#endif + struct __attribute__ ((packed)) PairingPublicKey + { + uint8_t code; + 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(); + if(connectionHandle>ATT_MAX_PEERS){ +#ifdef _BLE_TRACE_ + Serial.println("failed to find connection handle"); +#endif + break; + } + HCI.sendAclPkt(connectionHandle,SECURITY_CID,sizeof(PairingPublicKey),&pairingPublicKey); + uint8_t encryption = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::SENT_PUBKEY; + ATT.setPeerEncryption(connectionHandle, encryption); + + + uint8_t Z = 0; + + HCI.leRand(Nb); + HCI.leRand(&Nb[8]); + +#ifdef _BLE_TRACE_ + Serial.print("nb: "); + btct.printBytes(Nb, 16); +#endif + struct __attribute__ ((packed)) F4Params + { + uint8_t U[32]; + uint8_t V[32]; + uint8_t Z; + } f4Params = {0,0,Z}; + for(int i=0; i<32; i++){ + f4Params.U[31-i] = pairingPublicKey.publicKey[i]; + f4Params.V[31-i] = HCI.remotePublicKeyBuffer[i]; + } + + struct __attribute__ ((packed)) PairingConfirm + { + uint8_t code; + uint8_t cb[16]; + } pairingConfirm = {CONNECTION_PAIRING_CONFIRM,0}; + + btct.AES_CMAC(Nb,(unsigned char *)&f4Params,sizeof(f4Params),pairingConfirm.cb); + +#ifdef _BLE_TRACE_ + Serial.print("cb: "); + btct.printBytes(pairingConfirm.cb, 16); +#endif - if (leMetaHeader->subevent == EVT_LE_CONN_COMPLETE) { - struct __attribute__ ((packed)) EvtLeConnectionComplete { - uint8_t status; - uint16_t handle; - uint8_t role; - uint8_t peerBdaddrType; - uint8_t peerBdaddr[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 cb_temp[sizeof(pairingConfirm.cb)]; + for(int i=0; istatus,HEX); + for(int i=0; i<64; i++){ + Serial.print(" 0x"); + Serial.print(evtReadLocalP256Complete->localPublicKey[i],HEX); + } + Serial.println("."); +#endif + } + break; } - } else if (leMetaHeader->subevent == EVT_LE_ADVERTISING_REPORT) { - struct __attribute__ ((packed)) EvtLeAdvertisingReport { - uint8_t status; - uint8_t type; - uint8_t peerBdaddrType; - uint8_t peerBdaddr[6]; - uint8_t eirLength; - uint8_t eirData[31]; - } *leAdvertisingReport = (EvtLeAdvertisingReport*)&pdata[sizeof(HCIEventHdr) + sizeof(LeMetaEventHeader)]; - - if (leAdvertisingReport->status == 0x01) { - // last byte is RSSI - int8_t rssi = leAdvertisingReport->eirData[leAdvertisingReport->eirLength]; - - GAP.handleLeAdvertisingReport(leAdvertisingReport->type, - leAdvertisingReport->peerBdaddrType, - leAdvertisingReport->peerBdaddr, - leAdvertisingReport->eirLength, - leAdvertisingReport->eirData, - rssi); + case GENERATE_DH_KEY_COMPLETE:{ + struct __attribute__ ((packed)) EvtLeDHKeyComplete{ + uint8_t subEventCode; + uint8_t status; + uint8_t DHKey[32]; + } *evtLeDHKeyComplete = (EvtLeDHKeyComplete*)&pdata[sizeof(HCIEventHdr)]; + if(evtLeDHKeyComplete->status == 0x0){ +#ifdef _BLE_TRACE_ + Serial.println("DH key generated"); +#endif + uint16_t connectionHandle = ATT.getPeerEncrptingConnectionHandle(); + if(connectionHandle>ATT_MAX_PEERS){ +#ifdef _BLE_TRACE_ + Serial.println("Failed to find connection handle DH key check"); +#endif + break; + } + + + for(int i=0; i<32; i++) DHKey[31-i] = evtLeDHKeyComplete->DHKey[i]; + +#ifdef _BLE_TRACE_ + Serial.println("Stored our DHKey:"); + btct.printBytes(DHKey,32); +#endif + uint8_t encryption = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::DH_KEY_CALULATED; + ATT.setPeerEncryption(connectionHandle, encryption); + if((encryption & PEER_ENCRYPTION::RECEIVED_DH_CHECK) > 0){ +#ifdef _BLE_TRACE_ + Serial.println("Received DHKey check already so calculate f5, f6 now."); +#endif + L2CAPSignaling.smCalculateLTKandConfirm(connectionHandle, HCI.remoteDHKeyCheckBuffer); + + }else{ +#ifdef _BLE_TRACE_ + Serial.println("Waiting on other DHKey check before calculating."); +#endif + } + }else{ +#ifdef _BLE_TRACE_ + Serial.print("Key generation error: 0x"); + Serial.println(evtLeDHKeyComplete->status, HEX); +#endif + } + break; } + default: + { +#ifdef _BLE_TRACE_ + Serial.println("[Info] Unhandled meta event"); +#endif + } + } + }else{ +#ifdef _BLE_TRACE_ + Serial.println("[Info] Unhandled event"); +#endif + } +} +int HCIClass::leEncrypt(uint8_t* key, uint8_t* plaintext, uint8_t* status, uint8_t* ciphertext){ + struct __attribute__ ((packed)) LeEncryptCommand + { + uint8_t key[16]; + uint8_t plaintext[16]; + } leEncryptCommand = {0,0}; + for(int i=0; i<16; i++){ + leEncryptCommand.key[15-i] = key[i]; + leEncryptCommand.plaintext[15-i] = plaintext[i]; + } + + 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: "); + Serial.println(_cmdResponseLen); + Serial.println("."); + for(int i=0; i<20; i++){ + Serial.print(" 0x"); + Serial.print(_cmdResponse[i],HEX); } + Serial.println("."); +#endif + for(int i=0; i<16; i++){ + ciphertext[15-i] = _cmdResponse[i]; + } + return 1; + } +#ifdef _BLE_TRACE_ + Serial.print("Error with AES: 0x"); + Serial.println(res, HEX); +#endif + return res; +} +int HCIClass::leRand(uint8_t rand[]){ + int res = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::RANDOM); + if(res == 0){ + memcpy(rand,_cmdResponse, 8); /// backwards but it's a random number + } + return res; +} +int HCIClass::getLTK(uint8_t* address, uint8_t* LTK){ + if(_getLTK!=0){ + return _getLTK(address, LTK); + }else{ + return 0; + } +} +int HCIClass::storeIRK(uint8_t* address, uint8_t* IRK){ + if(_storeIRK!=0){ + return _storeIRK(address, IRK); + }else{ + return 0; + } +} +int HCIClass::storeLTK(uint8_t* address, uint8_t* LTK){ + if(_storeLTK!=0){ + return _storeLTK(address, LTK); + }else{ + 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(){ + // If get, set, IRK, LTK all set then we can bond. + AuthReq local = AuthReq(); + if(_storeIRK!=0 && _storeLTK!=0 && _getLTK!=0 && _getIRKs!=0){ + local.setBonding(true); } + local.setSC(true); + local.setMITM(true); + local.setCT2(true); + return LOCAL_AUTHREQ; } void HCIClass::dumpPkt(const char* prefix, uint8_t plen, uint8_t pdata[]) diff --git a/src/utility/HCI.h b/src/utility/HCI.h index 6d6b69ee..af46265a 100644 --- a/src/utility/HCI.h +++ b/src/utility/HCI.h @@ -21,6 +21,35 @@ #define _HCI_H_ #include +#include "bitDescriptions.h" + +#include "L2CAPSignaling.h" + +#define OGF_LINK_CTL 0x01 +#define OGF_HOST_CTL 0x03 +#define OGF_INFO_PARAM 0x04 +#define OGF_STATUS_PARAM 0x05 +#define OGF_LE_CTL 0x08 + +enum LE_COMMAND { + ENCRYPT = 0x0017, + RANDOM = 0x0018, + LONG_TERM_KEY_REPLY = 0x001A, + LONG_TERM_KEY_NEGATIVE_REPLY = 0x1B, + READ_LOCAL_P256 = 0x0025, + GENERATE_DH_KEY_V1 = 0x0026, + GENERATE_DH_KEY_V2 = 0x005E +}; +enum LE_META_EVENT { + CONN_COMPLETE = 0x01, + ADVERTISING_REPORT = 0x02, + LONG_TERM_KEY_REQUEST = 0x05, + REMOTE_CONN_PARAM_REQ = 0x06, + READ_LOCAL_P256_COMPLETE = 0x08, + GENERATE_DH_KEY_COMPLETE = 0x09 +}; +String metaEventToString(LE_META_EVENT event); +String commandToString(LE_COMMAND command); class HCIClass { public: @@ -36,12 +65,14 @@ class HCIClass { virtual int reset(); virtual int readLocalVersion(uint8_t& hciVer, uint16_t& hciRev, uint8_t& lmpVer, uint16_t& manufacturer, uint16_t& lmpSubVer); + virtual int readBdAddr(uint8_t addr[6]); + virtual int readBdAddr(); virtual int readRssi(uint16_t handle); virtual int setEventMask(uint64_t eventMask); - + virtual int setLeEventMask(uint64_t leEventMask); virtual int readLeBufferSize(uint16_t& pktLen, uint8_t& maxPkt); virtual int leSetRandomAddress(uint8_t addr[6]); virtual int leSetAdvertisingParameters(uint16_t minInterval, uint16_t maxInterval, @@ -62,7 +93,22 @@ class HCIClass { virtual int leConnUpdate(uint16_t handle, uint16_t minInterval, uint16_t maxInterval, uint16_t latency, uint16_t supervisionTimeout); virtual int leCancelConn(); - + virtual int leEncrypt(uint8_t* Key, uint8_t* plaintext, uint8_t* status, uint8_t* ciphertext); + // 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); + 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); @@ -71,8 +117,27 @@ class HCIClass { virtual void debug(Stream& stream); virtual void noDebug(); -private: + // 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]; + uint8_t DHKey[32]; + uint8_t localAddr[6]; + uint8_t LTK[16]; + virtual int getLTK(uint8_t* address, uint8_t* LTK); + virtual int storeLTK(uint8_t* address, uint8_t* LTK); + virtual int storeIRK(uint8_t* address, uint8_t* IRK); + int (*_storeIRK)(uint8_t* address, uint8_t* peerIrk) = 0; + 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: virtual void handleAclDataPkt(uint8_t plen, uint8_t pdata[]); virtual void handleNumCompPkts(uint16_t handle, uint16_t numPkts); diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index 3cca56cc..2ef08b62 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -18,16 +18,21 @@ */ #include "HCI.h" - +#include "ATT.h" +#include "btct.h" #include "L2CAPSignaling.h" - +#include "keyDistribution.h" +#include "bitDescriptions.h" #define CONNECTION_PARAMETER_UPDATE_REQUEST 0x12 #define CONNECTION_PARAMETER_UPDATE_RESPONSE 0x13 +//#define _BLE_TRACE_ + L2CAPSignalingClass::L2CAPSignalingClass() : _minInterval(0), _maxInterval(0), - _supervisionTimeout(0) + _supervisionTimeout(0), + _pairing_enabled(1) { } @@ -108,6 +113,331 @@ void L2CAPSignalingClass::handleData(uint16_t connectionHandle, uint8_t dlen, ui connectionParameterUpdateResponse(connectionHandle, identifier, length, data); } } +void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]) +{ + struct __attribute__ ((packed)) L2CAPSignalingHdr { + uint8_t code; + uint8_t data[64]; + } *l2capSignalingHdr = (L2CAPSignalingHdr*)data; +#ifdef _BLE_TRACE_ + Serial.print("dlen: "); + Serial.println(dlen); +#endif + uint8_t code = l2capSignalingHdr->code; + +#ifdef _BLE_TRACE_ + Serial.print("handleSecurityData: code: 0x"); + Serial.println(code, HEX); + Serial.print("rx security:"); + btct.printBytes(data,dlen); +#endif + if (code == CONNECTION_PAIRING_REQUEST) { + + if (isPairingEnabled()){ + if (_pairing_enabled >= 2) _pairing_enabled = 0; // 2 = pair once only + + // 0x1 + struct __attribute__ ((packed)) PairingRequest { + uint8_t ioCapability; + uint8_t oobDataFlag; + uint8_t authReq; + uint8_t maxEncSize; + uint8_t initiatorKeyDistribution; + uint8_t responderKeyDistribution; + } *pairingRequest = (PairingRequest*)l2capSignalingHdr->data; + + KeyDistribution responseKD = KeyDistribution(); + responseKD.setIdKey(true); + + ATT.remoteKeyDistribution = responseKD;// KeyDistribution(pairingRequest->initiatorKeyDistribution); + ATT.localKeyDistribution = responseKD; //KeyDistribution(pairingRequest->responderKeyDistribution); + // KeyDistribution rkd(pairingRequest->responderKeyDistribution); + AuthReq req(pairingRequest->authReq); +#ifdef _BLE_TRACE_ + Serial.print("Req has properties: "); + Serial.print(req.Bonding()?"bonding, ":"no bonding, "); + Serial.print(req.CT2()?"CT2, ":"no CT2, "); + Serial.print(req.KeyPress()?"KeyPress, ":"no KeyPress, "); + Serial.print(req.MITM()?"MITM, ":"no MITM, "); + Serial.print(req.SC()?"SC, ":"no SC, "); +#endif + + 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.println(ATT.getPeerEncryption(connectionHandle), BIN); +#endif + struct __attribute__ ((packed)) PairingResponse { + uint8_t code; + uint8_t ioCapability; + uint8_t oobDataFlag; + uint8_t authReq; + uint8_t maxEncSize; + uint8_t initiatorKeyDistribution; + uint8_t responderKeyDistribution; + } response = { CONNECTION_PAIRING_RESPONSE, HCI.localIOCap(), 0, HCI.localAuthreq().getOctet(), 0x10, responseKD.getOctet(), responseKD.getOctet()}; + + HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); + + } else { + // Pairing not enabled + uint8_t ret[2] = {CONNECTION_PAIRING_FAILED, 0x05}; // reqect pairing + HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(ret), ret); + ATT.setPeerEncryption(connectionHandle, NO_ENCRYPTION); + } + } + else if (code == CONNECTION_PAIRING_RANDOM) + { + struct __attribute__ ((packed)) PairingRandom { + uint8_t Na[16]; + } *pairingRandom = (PairingRandom*)l2capSignalingHdr->data; + for(int i=0; i<16; i++){ + HCI.Na[15-i] = pairingRandom->Na[i]; + } +#ifdef _BLE_TRACE_ + Serial.println("[Info] Pairing random."); +#endif + struct __attribute__ ((packed)) PairingResponse { + uint8_t code; + uint8_t Nb[16]; + } response = { CONNECTION_PAIRING_RANDOM, 0}; + 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) + { + } + else if(code == CONNECTION_PAIRING_FAILED) + { + struct __attribute__ ((packed)) PairingFailed + { + uint8_t code; + uint8_t reason; + } *pairingFailed = (PairingFailed*)data; +#ifdef _BLE_TRACE_ + 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 { + 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 { + uint8_t x[32]; + uint8_t y[32]; + } *connectionPairingPublicKey = (ConnectionPairingPublicKey*)l2capSignalingHdr->data; + struct __attribute__ ((packed)) GenerateDHKeyCommand { + uint8_t x[32]; + uint8_t y[32]; + } generateDHKeyCommand = { + 0x00, + 0x00, + }; + memcpy(generateDHKeyCommand.x,connectionPairingPublicKey->x,32); + memcpy(generateDHKeyCommand.y,connectionPairingPublicKey->y,32); + struct __attribute__ ((packed)) ReadPublicKeyCommand { + uint8_t code; + } readPublicKeyCommand = { + LE_COMMAND::READ_LOCAL_P256, + }; + + 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."); +#endif + }else{ +#ifdef _BLE_TRACE_ + Serial.println("[Info] Pairing public key"); + Serial.print("Failed to store encryption request with handle: 0x"); + Serial.println(connectionHandle,HEX); +#endif + } + + memcpy(HCI.remotePublicKeyBuffer,&generateDHKeyCommand,sizeof(generateDHKeyCommand)); + HCI.sendCommand( (OGF_LE_CTL << 10 )| LE_COMMAND::READ_LOCAL_P256, 0); + } + else if(code == CONNECTION_PAIRING_DHKEY_CHECK) + { + uint8_t RemoteDHKeyCheck[16]; + for(int i=0; i<16; i++) RemoteDHKeyCheck[15-i] = l2capSignalingHdr->data[i]; + + +#ifdef _BLE_TRACE_ + Serial.println("[Info] DH Key check"); + Serial.print("Remote DHKey Check: "); + btct.printBytes(RemoteDHKeyCheck, 16); +#endif + + + + uint8_t encryptionState = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::RECEIVED_DH_CHECK; + ATT.setPeerEncryption(connectionHandle, encryptionState); + if((encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED) == 0){ +#ifdef _BLE_TRACE_ + Serial.println("DHKey not yet ready, will calculate f5, f6 later"); +#endif + // store RemoteDHKeyCheck for later check + memcpy(HCI.remoteDHKeyCheckBuffer,RemoteDHKeyCheck,16); + + } else { + // We've already calculated the DHKey so we can calculate our check and send it. + smCalculateLTKandConfirm(connectionHandle, RemoteDHKeyCheck); + + } + } +} + +void L2CAPSignalingClass::smCalculateLTKandConfirm(uint16_t handle, uint8_t expectedEa[]) +{ // Authentication stage 2: LTK Calculation + + uint8_t localAddress[7]; + uint8_t remoteAddress[7]; + ATT.getPeerAddrWithType(handle, remoteAddress); + + HCI.readBdAddr(); + memcpy(&localAddress[1],HCI.localAddr,6); + localAddress[0] = 0; // IOT 33 uses a static address // TODO: confirm for Nano BLE + + // Compute the LTK and MacKey + uint8_t MacKey[16]; + btct.f5(HCI.DHKey, HCI.Na, HCI.Nb, remoteAddress, localAddress, MacKey, HCI.LTK); + + // Compute Ea and Eb + uint8_t Ea[16]; + uint8_t Eb[16]; + uint8_t R[16]; + uint8_t MasterIOCap[3]; + uint8_t SlaveIOCap[3] = {HCI.localAuthreq().getOctet(), 0x0, HCI.localIOCap()}; + + ATT.getPeerIOCap(handle, MasterIOCap); + for(int i=0; i<16; i++) R[i] = 0; + + btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, remoteAddress, localAddress, Ea); + btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, remoteAddress, Eb); + +#ifdef _BLE_TRACE_ + Serial.println("Calculate and confirm LTK via f5, f6:"); + Serial.print("DHKey : "); btct.printBytes(HCI.DHKey,32); + Serial.print("Na : "); btct.printBytes(HCI.Na,16); + Serial.print("Nb : "); btct.printBytes(HCI.Nb,16); + Serial.print("MacKey : "); btct.printBytes(MacKey,16); + Serial.print("LTK : "); btct.printBytes(HCI.LTK,16); + Serial.print("Expected Ea: "); btct.printBytes(expectedEa, 16); + Serial.print("Ea : "); btct.printBytes(Ea, 16); + Serial.print("Eb : "); btct.printBytes(Eb,16); + Serial.print("Local Addr : "); btct.printBytes(localAddress, 7); + Serial.print("LocalIOCap : "); btct.printBytes(SlaveIOCap, 3); + Serial.print("MasterAddr : "); btct.printBytes(remoteAddress, 7); + Serial.print("MasterIOCAP: "); btct.printBytes(MasterIOCap, 3); +#endif + + // Check if Ea = expectedEa + if (memcmp(Ea, expectedEa, 16) == 0){ + // Check ok + // Send our confirmation value to complete authentication stage 2 + uint8_t ret[17]; + ret[0] = CONNECTION_PAIRING_DHKEY_CHECK; + for(int i=0; i 0; +} + void L2CAPSignalingClass::connectionParameterUpdateRequest(uint16_t handle, uint8_t identifier, uint8_t dlen, uint8_t data[]) { struct __attribute__ ((packed)) L2CAPConnectionParameterUpdateRequest { diff --git a/src/utility/L2CAPSignaling.h b/src/utility/L2CAPSignaling.h index 233eca7f..26042167 100644 --- a/src/utility/L2CAPSignaling.h +++ b/src/utility/L2CAPSignaling.h @@ -23,6 +23,33 @@ #include #define SIGNALING_CID 0x0005 +#define SECURITY_CID 0x0006 + + +#define CONNECTION_PAIRING_REQUEST 0x01 +#define CONNECTION_PAIRING_RESPONSE 0x02 +#define CONNECTION_PAIRING_CONFIRM 0x03 +#define CONNECTION_PAIRING_RANDOM 0x04 +#define CONNECTION_PAIRING_FAILED 0x05 +#define CONNECTION_ENCRYPTION_INFORMATION 0x06 +#define CONNECTION_MASTER_IDENTIFICATION 0x07 +#define CONNECTION_IDENTITY_INFORMATION 0x08 +#define CONNECTION_IDENTITY_ADDRESS 0x09 +#define CONNECTION_SIGNING_INFORMATION 0x0A +#define CONNECTION_SECURITY_REQUEST 0x0B +#define CONNECTION_PAIRING_PUBLIC_KEY 0x0C +#define CONNECTION_PAIRING_DHKEY_CHECK 0x0D +#define CONNECTION_PAIRING_KEYPRESS 0x0E + +#define IOCAP_DISPLAY_ONLY 0x00 +#define IOCAP_DISPLAY_YES_NO 0x01 +#define IOCAP_KEYBOARD_ONLY 0x02 +#define IOCAP_NO_INPUT_NO_OUTPUT 0x03 +#define IOCAP_KEYBOARD_DISPLAY 0x04 + + +#define LOCAL_AUTHREQ 0b00101101 +// #define LOCAL_IOCAP IOCAP_DISPLAY_ONLY // will use JustWorks pairing class L2CAPSignalingClass { public: @@ -36,20 +63,32 @@ class L2CAPSignalingClass { virtual void handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); + virtual void handleSecurityData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); + virtual void removeConnection(uint8_t handle, uint16_t reason); virtual void setConnectionInterval(uint16_t minInterval, uint16_t maxInterval); virtual void setSupervisionTimeout(uint16_t supervisionTimeout); + + virtual void setPairingEnabled(uint8_t enabled); + virtual bool isPairingEnabled(); + + + + virtual void smCalculateLTKandConfirm(uint16_t handle, uint8_t expectedEa[]); + private: virtual void connectionParameterUpdateRequest(uint16_t handle, uint8_t identifier, uint8_t dlen, uint8_t data[]); virtual void connectionParameterUpdateResponse(uint16_t handle, uint8_t identifier, uint8_t dlen, uint8_t data[]); + private: uint16_t _minInterval; uint16_t _maxInterval; uint16_t _supervisionTimeout; + uint8_t _pairing_enabled; }; extern L2CAPSignalingClass& L2CAPSignaling; diff --git a/src/utility/bitDescriptions.cpp b/src/utility/bitDescriptions.cpp new file mode 100644 index 00000000..bf896bc1 --- /dev/null +++ b/src/utility/bitDescriptions.cpp @@ -0,0 +1,30 @@ +#include "bitDescriptions.h" + + +#define BONDING_BIT 0b00000001 +#define MITM_BIT 0b00000100 +#define SC_BIT 0b00001000 +#define KEYPRESS_BIT 0b00010000 +#define CT2_BIT 0b00100000 + + +AuthReq::AuthReq(){} +AuthReq::AuthReq(uint8_t octet):_octet(octet){} +bool AuthReq::Bonding(){ return (_octet & BONDING_BIT)>0;} +bool AuthReq::MITM(){ return (_octet & MITM_BIT)>0;} +bool AuthReq::SC(){ return (_octet & SC_BIT)>0;} +bool AuthReq::KeyPress(){ return (_octet & KEYPRESS_BIT)>0;} +bool AuthReq::CT2(){ return (_octet & CT2_BIT)>0;} + + +void AuthReq::setBonding(bool state) { _octet= state? _octet|BONDING_BIT : _octet&~BONDING_BIT;} +void AuthReq::setMITM(bool state) { _octet= state? _octet|MITM_BIT : _octet&~MITM_BIT;} +void AuthReq::setSC(bool state){ _octet= state? _octet|SC_BIT : _octet&~SC_BIT;} +void AuthReq::setKeyPress(bool state){ _octet= state? _octet|KEYPRESS_BIT : _octet&~KEYPRESS_BIT;} +void AuthReq::setCT2(bool state){ _octet= state? _octet|CT2_BIT : _octet&~CT2_BIT;} + +uint8_t _octet; + + +void AuthReq::setOctet( uint8_t octet){_octet = octet;} +uint8_t AuthReq::getOctet() {return _octet;} diff --git a/src/utility/bitDescriptions.h b/src/utility/bitDescriptions.h new file mode 100644 index 00000000..6d32c52a --- /dev/null +++ b/src/utility/bitDescriptions.h @@ -0,0 +1,41 @@ +#ifndef _BIT_DESCRIPTIONS_H_ +#define _BIT_DESCRIPTIONS_H_ +#include + +class AuthReq{ +public: + AuthReq(); + AuthReq(uint8_t octet); + void setOctet( uint8_t octet); + uint8_t getOctet(); + + + // The Bonding_Flags field is a 2-bit field that indicates the type of bonding being requested by the initiating device + bool Bonding(); + // The MITM field is a 1-bit flag that is set to one if the device is requesting MITM protection + bool MITM(); + // The SC field is a 1 bit flag. If LE Secure Connections pairing is supported by the device, then the SC field shall be set to 1, otherwise it shall be set to 0. + bool SC(); + // The keypress field is a 1-bit flag that is used only in the Passkey Entry protocol and shall be ignored in other protocols. + bool KeyPress(); + // The CT2 field is a 1-bit flag that shall be set to 1 upon transmission to indicate support for the h7 function. + bool CT2(); + + void setBonding(bool state); + void setMITM(bool state); + void setSC(bool state); + void setKeyPress(bool state); + void setCT2(bool state); +private: + uint8_t _octet; +}; + +enum IOCap { + DisplayOnly, + DisplayYesNo, + KeyboardOnly, + NoInputNoOutput, + KeyboardDisplay +}; + +#endif \ No newline at end of file diff --git a/src/utility/btct.cpp b/src/utility/btct.cpp new file mode 100644 index 00000000..b4faf053 --- /dev/null +++ b/src/utility/btct.cpp @@ -0,0 +1,390 @@ +#include "btct.h" +#include +#include "HCI.h" +#include "ArduinoBLE.h" +BluetoothCryptoToolbox::BluetoothCryptoToolbox(){} +// In step 1, AES-128 with key K is applied to an all-zero input block. +// In step 2, K1 is derived through the following operation: +// If the most significant bit of L is equal to 0, K1 is the left-shift +// of L by 1 bit. +// Otherwise, K1 is the exclusive-OR of const_Rb and the left-shift of L +// by 1 bit. +// In step 3, K2 is derived through the following operation: +// If the most significant bit of K1 is equal to 0, K2 is the left-shift +// of K1 by 1 bit. +// Otherwise, K2 is the exclusive-OR of const_Rb and the left-shift of +// K1 by 1 bit. +// In step 4, (K1,K2) := Generate_Subkey(K) is returned. +unsigned char const_Rb[16] = { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x87 + }; + +#define DHKEY_LENGTH 32 +#define N_LEN 16 +#define ADDR_LEN 6 +#define LEN_LTK 16 +#define LEN_MAC_KEY 16 + +void BluetoothCryptoToolbox::printBytes(uint8_t bytes[], uint8_t length){ + for(int i=0; i0){ + Serial.print(", 0x"); + }else{ + Serial.print("0x"); + } + Serial.print(bytes[i],HEX); + } + Serial.print('\n'); +} +int BluetoothCryptoToolbox::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[]) +{ + uint8_t SALT[16] = {0x6C, 0x88, 0x83, 0x91, 0xAA, 0xF5, 0xA5, 0x38, 0x60, 0x37, 0x0B, 0xDB, 0x5A, 0x60, 0x83, 0xBE}; + uint8_t keyID[4] = {0x62, 0x74, 0x6c, 0x65}; + uint8_t length[2]; + length[0] = 0x01; + length[1] = 0x00; +#ifdef _BLE_TRACE_ + Serial.print("Starting f5 calculation"); + Serial.print("Using DHKey: "); + printBytes(DHKey, DHKEY_LENGTH); + Serial.print("Using N_Master: "); + printBytes(N_master, N_LEN); + Serial.print("Using N_Slave: "); + printBytes(N_slave, N_LEN); + + Serial.println("Using BD_ADDR_MASTER: "); + printBytes(BD_ADDR_master, ADDR_LEN); + Serial.println("Using BD_ADDR_SLAVE: "); + printBytes(BD_ADDR_slave, ADDR_LEN); +#endif + + uint8_t ADD_M[7]; + uint8_t ADD_S[7]; + uint8_t T[16]; + + for(int i=0; i<6; i++){ + ADD_M[1+i] = BD_ADDR_master[i]; + ADD_M[0] = 0x00; + ADD_S[i+1] = BD_ADDR_slave[i]; + ADD_S[0] = 0x00; + } + struct __attribute__ ((packed)) CmacInput + { + uint8_t counter; + uint8_t keyID[4]; + uint8_t N1[16]; + uint8_t N2[16]; + uint8_t A1[7]; + uint8_t A2[7]; + uint8_t length[2]; + } cmacInput = {0,0,0,0,0,0,0}; + cmacInput.counter = 0; + memcpy(cmacInput.keyID, keyID, 4); + memcpy(cmacInput.N1,N_master,16); + memcpy(cmacInput.N2,N_slave,16); + memcpy(cmacInput.A1,BD_ADDR_master,7); + memcpy(cmacInput.A2,BD_ADDR_slave,7); + memcpy(cmacInput.length,length,2); + AES_CMAC(SALT, DHKey, 32, T); + + AES_CMAC(T, (uint8_t*)&cmacInput,sizeof(cmacInput), MacKey); + cmacInput.counter=1; + AES_CMAC(T, (uint8_t*)&cmacInput, sizeof(cmacInput), LTK); + + return 1; +} +int BluetoothCryptoToolbox::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[]) +{ + struct __attribute__ ((packed)) F6Input + { + uint8_t N1[16]; + uint8_t N2[16]; + uint8_t R[16]; + uint8_t IOCap[3]; + uint8_t A1[7]; + uint8_t A2[7]; + } f6Input = {0,0,0,0,0,0}; + + memcpy(f6Input.N1, N1, 16); + memcpy(f6Input.N2, N2, 16); + memcpy(f6Input.R, R, 16); + memcpy(f6Input.IOCap, IOCap, 3); + memcpy(f6Input.A1, A1, 7); + memcpy(f6Input.A2, A2, 7); + + + AES_CMAC(W, (uint8_t*)&f6Input, sizeof(f6Input),Ex); + 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); +} + +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 ) +{ + unsigned char X[16],Y[16], M_last[16], padded[16]; + unsigned char K1[16], K2[16]; + int n, i, flag; + generateSubkey(key,K1,K2); + + n = (length+15) / 16; /* n is number of rounds */ + + if ( n == 0 ) { + n = 1; + flag = 0; + } else { + if ( (length%16) == 0 ) { /* last block is a complete block */ + flag = 1; + } else { /* last block is not complete block */ + flag = 0; + } + } + + if ( flag ) { /* last block is complete block */ + xor_128(&input[16*(n-1)],K1,M_last); + } else { + padding(&input[16*(n-1)],padded,length%16); + xor_128(padded,K2,M_last); + } + + for ( i=0; i<16; i++ ) X[i] = 0; + for ( i=0; i=0; i-- ) { + output[i] = input[i] << 1; + output[i] |= overflow; + overflow = (input[i] & 0x80)?1:0; + } + return; +} +// From RFC +void BluetoothCryptoToolbox::xor_128(unsigned char *a, unsigned char *b, unsigned char *out) +{ + int i; + for (i=0;i<16; i++) + { + out[i] = a[i] ^ b[i]; + } +} +BluetoothCryptoToolbox btct; \ No newline at end of file diff --git a/src/utility/btct.h b/src/utility/btct.h new file mode 100644 index 00000000..08f8f192 --- /dev/null +++ b/src/utility/btct.h @@ -0,0 +1,30 @@ +#ifndef _BTCT_H_ +#define _BTCT_H_ +#include + +// Implementation of functions defined in BTLE standard +class BluetoothCryptoToolbox{ +public: + BluetoothCryptoToolbox(); + void printBytes(uint8_t bytes[], uint8_t length); + void generateSubkey(uint8_t* K, uint8_t* K1, uint8_t* K2); + void AES_CMAC ( unsigned char *key, unsigned char *input, int length, + unsigned char *mac ); + 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); + void xor_128(unsigned char *a, unsigned char *b, unsigned char *out); + void padding ( unsigned char *lastb, unsigned char *pad, int length ); +}; +extern BluetoothCryptoToolbox btct; +#endif \ No newline at end of file diff --git a/src/utility/keyDistribution.cpp b/src/utility/keyDistribution.cpp new file mode 100644 index 00000000..f754366c --- /dev/null +++ b/src/utility/keyDistribution.cpp @@ -0,0 +1,24 @@ +#include "keyDistribution.h" + +KeyDistribution::KeyDistribution():_octet(0){} +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