From c66379882f727276b74c319c1ec9fa824fc96bf1 Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Fri, 1 Jan 2021 11:22:34 +0000 Subject: [PATCH 01/13] Initial - encrypted read --- src/ArduinoBLE.h | 1 + src/BLEProperty.h | 40 ++- src/local/BLELocalDevice.cpp | 4 + src/local/BLELocalDevice.h | 1 + src/utility/ATT.cpp | 132 +++++++- src/utility/ATT.h | 23 +- src/utility/HCI.cpp | 571 ++++++++++++++++++++++++++++----- src/utility/HCI.h | 37 ++- src/utility/L2CAPSignaling.cpp | 204 +++++++++++- src/utility/L2CAPSignaling.h | 22 ++ src/utility/btct.cpp | 322 +++++++++++++++++++ src/utility/btct.h | 26 ++ 12 files changed, 1301 insertions(+), 82 deletions(-) create mode 100644 src/utility/btct.cpp create mode 100644 src/utility/btct.h 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/BLEProperty.h b/src/BLEProperty.h index 6cdd888f..aeee4c85 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,43 @@ enum BLEProperty { BLEWriteWithoutResponse = 0x04, BLEWrite = 0x08, BLENotify = 0x10, - BLEIndicate = 0x20 + BLEIndicate = 0x20, + BLEAuth = 1 << 6, + BLEExtProp = 1 << 7, +}; + +#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/local/BLELocalDevice.cpp b/src/local/BLELocalDevice.cpp index 57477e23..e83c4b0d 100644 --- a/src/local/BLELocalDevice.cpp +++ b/src/local/BLELocalDevice.cpp @@ -107,6 +107,10 @@ int BLELocalDevice::begin() end(); return 0; } + if (HCI.setLeEventMask(0x00000000000001FF) != 0) { + end(); + return 0; + } uint16_t pktLen; uint8_t maxPkt; diff --git a/src/local/BLELocalDevice.h b/src/local/BLELocalDevice.h index 20837c14..1a927420 100644 --- a/src/local/BLELocalDevice.h +++ b/src/local/BLELocalDevice.h @@ -74,6 +74,7 @@ class BLELocalDevice { void debug(Stream& stream); void noDebug(); + uint8_t BDaddress[6]; private: }; diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index 2696960e..af0129a7 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -95,6 +95,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)); @@ -267,12 +268,22 @@ 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"); +#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 +292,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 +307,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 +336,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 +366,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 +421,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; @@ -807,6 +834,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); @@ -819,7 +854,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); @@ -906,6 +944,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)]; @@ -962,6 +1002,11 @@ 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->properties() & BLEAuth) > 0 && (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES)==0) { + holdResponse = true; + sendError(connectionHandle, opcode, handle, ATT_ECODE_INSUFF_ENC); + } uint16_t valueLength = characteristic->valueLength(); @@ -994,8 +1039,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[]) @@ -1687,4 +1736,81 @@ 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 ", 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 +521,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 +551,9 @@ void HCIClass::handleAclDataPkt(uint8_t /*plen*/, uint8_t pdata[]) uint16_t cid; } *aclHdr = (HCIACLHdr*)pdata; +#ifdef _BLE_TRACE_ + Serial.println("Received data"); +#endif uint16_t aclFlags = (aclHdr->handle & 0xf000) >> 12; if ((aclHdr->dlen - 4) != aclHdr->len) { @@ -530,8 +586,18 @@ 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 + L2CAPSignaling.handleSecurityData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]); + + }else { struct __attribute__ ((packed)) { uint8_t op; uint8_t id; @@ -540,6 +606,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 +630,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,99 +647,447 @@ 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){ + ATT.setPeerEncryption(encryptionChange->connectionHandle, PEER_ENCRYPTION::ENCRYPTED_AES); + }else{ + ATT.setPeerEncryption(encryptionChange->connectionHandle, PEER_ENCRYPTION::NO_ENCRYPTION); + } + if(ATT.holdBufferSize>0){ + HCI.sendAclPkt(encryptionChange->connectionHandle, ATT_CID, ATT.holdBufferSize, ATT.holdBuffer); + ATT.holdBufferSize = 0; + } + } + 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 == 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 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); + } + 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 - 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); + // Send our LTK back + struct __attribute__ ((packed)) LTKReply + { + uint16_t connectionHandle; + uint8_t LTK[16]; + } ltkReply = {0,0}; + ltkReply.connectionHandle = ATT.getPeerEncrptingConnectionHandle(); + 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 + } + 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 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); + + // 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; + for(int i=0; i<16; i++){ + Nb[i] = rand(); //// Should use ESP or ECCx08 + } +#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 + 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; + } + 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; + } + + uint8_t encryption = ATT.getPeerEncryption(connectionHandle); + + 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 + encryption |= PEER_ENCRYPTION::DH_KEY_CALULATED; + ATT.setPeerEncryption(connectionHandle, encryption); +#ifdef _BLE_TRACE_ + if(encryption | PEER_ENCRYPTION::RECEIVED_DH_CHECK){ + Serial.println("Recieved DHKey check already so calculate f5, f6."); + }else{ + Serial.println("Waiting on other DHKey check before calculating."); + } + }else{ + 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, sizeof(leEncryptCommand), &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; +} 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 86ec679c..c58b7572 100644 --- a/src/utility/HCI.h +++ b/src/utility/HCI.h @@ -22,6 +22,30 @@ #include +#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, + LONG_TERM_KEY_REPLY = 0x001A, + 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: HCIClass(); @@ -37,10 +61,12 @@ class HCIClass { int readLocalVersion(uint8_t& hciVer, uint16_t& hciRev, uint8_t& lmpVer, uint16_t& manufacturer, uint16_t& lmpSubVer); int readBdAddr(uint8_t addr[6]); + int readBdAddr(); int readRssi(uint16_t handle); int setEventMask(uint64_t eventMask); + int setLeEventMask(uint64_t leEventMask); int readLeBufferSize(uint16_t& pktLen, uint8_t& maxPkt); int leSetRandomAddress(uint8_t addr[6]); @@ -62,7 +88,7 @@ class HCIClass { int leConnUpdate(uint16_t handle, uint16_t minInterval, uint16_t maxInterval, uint16_t latency, uint16_t supervisionTimeout); int leCancelConn(); - + int leEncrypt(uint8_t* Key, uint8_t* plaintext, uint8_t* status, uint8_t* ciphertext); int sendAclPkt(uint16_t handle, uint8_t cid, uint8_t plen, void* data); @@ -71,8 +97,15 @@ class HCIClass { void debug(Stream& stream); void noDebug(); -private: + // TODO: Send command be private again & use ATT implementation within ATT. int sendCommand(uint16_t opcode, uint8_t plen = 0, void* parameters = NULL); + uint8_t remotePublicKeyBuffer[64]; + uint8_t Na[16]; + uint8_t Nb[16]; + uint8_t DHKey[32]; + uint8_t localAddr[6]; + uint8_t LTK[16]; +private: void handleAclDataPkt(uint8_t plen, uint8_t pdata[]); void handleNumCompPkts(uint16_t handle, uint16_t numPkts); diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index f650cab2..45208514 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -18,7 +18,8 @@ */ #include "HCI.h" - +#include "ATT.h" +#include "btct.h" #include "L2CAPSignaling.h" #define CONNECTION_PARAMETER_UPDATE_REQUEST 0x12 @@ -97,6 +98,207 @@ 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) { + // 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; + + uint8_t peerIOCap[3]; + peerIOCap[0] = pairingRequest->authReq; + peerIOCap[1] = pairingRequest->oobDataFlag; + peerIOCap[2] = pairingRequest->ioCapability; + ATT.setPeerIOCap(connectionHandle, peerIOCap); + + 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, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 0x10, 0b1011, 0b1011}; + + HCI.sendAclPkt(connectionHandle, 0x06, sizeof(response), &response); + } + 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, 0x06, sizeof(response), &response); + } + 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 + } + 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,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]; + uint8_t BD_ADDR_REMOTE[7]; + ATT.getPeerAddrWithType(connectionHandle, BD_ADDR_REMOTE); + for(int i=0; i<16; i++) RemoteDHKeyCheck[15-i] = l2capSignalingHdr->data[i]; + uint8_t encryptionState = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::RECEIVED_DH_CHECK; + + +#ifdef _BLE_TRACE_ + Serial.println("[Info] DH Key check"); + Serial.print("Remote DHKey Check: "); + btct.printBytes(RemoteDHKeyCheck, 16); +#endif + + HCI.readBdAddr(); + ATT.setPeerEncryption(connectionHandle, encryptionState); + if(encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED > 0){ + // We've already calculated the DHKey so we can calculate our check and send it. + + uint8_t MacKey[16]; + uint8_t localAddress[7]; + + memcpy(&localAddress[1],HCI.localAddr,6); + localAddress[0] = 0; // IOT 33 uses a static address + + btct.f5(HCI.DHKey,HCI.Na,HCI.Nb,BD_ADDR_REMOTE,localAddress,MacKey,HCI.LTK); + + uint8_t Ea[16]; + uint8_t Eb[16]; + uint8_t R[16]; + uint8_t MasterIOCap[3]; + uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP}; + + ATT.getPeerIOCap(connectionHandle, MasterIOCap); + for(int i=0; i<16; i++) R[i] = 0; + + btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, BD_ADDR_REMOTE, localAddress, Ea); + btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, BD_ADDR_REMOTE, Eb); + + +#ifdef _BLE_TRACE_ + Serial.println("Calculate f5, f6:"); + Serial.print("DH : "); + btct.printBytes(HCI.DHKey,32); + Serial.println("Na : "); + btct.printBytes(HCI.Na,16); + Serial.println("Nb : "); + btct.printBytes(HCI.Nb,16); + Serial.print("MAC : "); + btct.printBytes(MacKey,16); + // Serial.print("Expected MAC: "); + // printBytes(EXPECTED_MAC, 16); + Serial.print("LTK : "); + btct.printBytes(HCI.LTK,16); + // Serial.print("Expected LTK: "); + // printBytes(EXPECTED_LTK, 16); + Serial.print("Expected Ex : "); + btct.printBytes(RemoteDHKeyCheck, 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(BD_ADDR_REMOTE, 7); + Serial.print("MasterIOCAP : "); + btct.printBytes(MasterIOCap, 3); + Serial.println("Send Eb Back."); +#endif + + uint8_t ret[17]; + ret[0] = 0x0d; + for(int i=0; i #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 LOCAL_AUTHREQ 0b00101101 +#define LOCAL_IOCAP 0x3 class L2CAPSignalingClass { public: @@ -36,6 +56,8 @@ class L2CAPSignalingClass { void handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); + void handleSecurityData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); + void removeConnection(uint8_t handle, uint16_t reason); void setConnectionInterval(uint16_t minInterval, uint16_t maxInterval); diff --git a/src/utility/btct.cpp b/src/utility/btct.cpp new file mode 100644 index 00000000..8fad5173 --- /dev/null +++ b/src/utility/btct.cpp @@ -0,0 +1,322 @@ +#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 +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..87e515af --- /dev/null +++ b/src/utility/btct.h @@ -0,0 +1,26 @@ +#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[]); + void test(); + void testF5(); + void testF6(); +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 From 97571e37dfb16f84db83050d638af61f6b37f368 Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Fri, 1 Jan 2021 14:41:48 +0000 Subject: [PATCH 02/13] With write encryptionn requirement --- src/utility/ATT.cpp | 59 +++++++++++++++++++++++++++++++++++++++++++-- src/utility/ATT.h | 3 +++ src/utility/HCI.cpp | 16 +++++++++--- 3 files changed, 72 insertions(+), 6 deletions(-) diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index 4c755615..5bd5e2aa 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -276,6 +276,8 @@ void ATTClass::handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[ 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; @@ -559,6 +561,7 @@ bool ATTClass::handleNotify(uint16_t handle, const uint8_t* value, int length) memcpy(¬ification[notificationLength], value, length); notificationLength += length; + /// TODO: Set encyption requirement on notify. HCI.sendAclPkt(_peers[i].connectionHandle, ATT_CID, notificationLength, notification); numNotifications++; @@ -1214,6 +1217,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; @@ -1226,10 +1230,33 @@ void ATTClass::writeReqOrCmd(uint16_t connectionHandle, uint16_t mtu, uint8_t op } return; } + // Check permssion + if((characteristic->properties() & BLEProperty::BLEAuth)> 0 && (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES) == 0){ + 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; } } @@ -1276,8 +1303,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[]) diff --git a/src/utility/ATT.h b/src/utility/ATT.h index a7d802d1..09d396ca 100644 --- a/src/utility/ATT.h +++ b/src/utility/ATT.h @@ -95,7 +95,10 @@ class ATTClass { virtual int setPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[]); virtual int getPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[]); uint8_t holdBuffer[64]; + uint8_t writeBuffer[64]; uint8_t holdBufferSize; + uint8_t writeBufferSize; + virtual int processWriteBuffer(); 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[]); diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index bd02764b..805a2c3d 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -666,13 +666,20 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) #endif if(encryptionChange->enabled>0){ 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); } - if(ATT.holdBufferSize>0){ - HCI.sendAclPkt(encryptionChange->connectionHandle, ATT_CID, ATT.holdBufferSize, ATT.holdBuffer); - ATT.holdBufferSize = 0; - } } else if (eventHdr->evt == EVT_CMD_COMPLETE) { @@ -940,6 +947,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) uint8_t Z = 0; for(int i=0; i<16; i++){ + /// TODO: Implement secure random Nb[i] = rand(); //// Should use ESP or ECCx08 } #ifdef _BLE_TRACE_ From fa42bb26cf49216b359999df944df4b868ae137a Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Sat, 2 Jan 2021 20:15:00 +0000 Subject: [PATCH 03/13] Write encryption & visible LTK / IRK --- .../EncryptedBatteryMonitor.ino | 209 ++++++++++++ src/BLECharacteristic.cpp | 8 +- src/BLECharacteristic.h | 4 +- src/BLEProperty.h | 11 +- src/BLETypedCharacteristic.h | 6 +- src/BLETypedCharacteristics.cpp | 28 +- src/BLETypedCharacteristics.h | 28 +- src/local/BLELocalCharacteristic.cpp | 18 +- src/local/BLELocalCharacteristic.h | 6 +- src/local/BLELocalDevice.cpp | 69 +++- src/local/BLELocalDevice.h | 7 +- src/remote/BLERemoteCharacteristic.cpp | 5 +- src/remote/BLERemoteCharacteristic.h | 4 +- src/utility/ATT.cpp | 45 ++- src/utility/ATT.h | 19 +- src/utility/HCI.cpp | 315 +++++++++++++++++- src/utility/HCI.h | 17 + src/utility/L2CAPSignaling.cpp | 45 ++- src/utility/btct.cpp | 32 ++ src/utility/btct.h | 2 + src/utility/keyDistribution.cpp | 24 ++ src/utility/keyDistribution.h | 29 ++ 22 files changed, 855 insertions(+), 76 deletions(-) create mode 100644 examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino create mode 100644 src/utility/keyDistribution.cpp create mode 100644 src/utility/keyDistribution.h diff --git a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino new file mode 100644 index 00000000..338a374e --- /dev/null +++ b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino @@ -0,0 +1,209 @@ +/* + Battery Monitor + + This example creates a BLE peripheral with the standard battery service and + level characteristic. The A0 pin is used to calculate the battery level. + + The circuit: + - Arduino MKR WiFi 1010, Arduino Uno WiFi Rev2 board, Arduino Nano 33 IoT, + Arduino Nano 33 BLE, or Arduino Nano 33 BLE Sense board. + + You can use a generic BLE central app, like LightBlue (iOS and Android) or + nRF Connect (Android), to interact with the services and characteristics + created in this sketch. + + This example code is in the public domain. +*/ + +#include + + + // BLE Battery Service +BLEService batteryService("180F"); + +// BLE Battery Level Characteristic +BLEUnsignedCharCharacteristic batteryLevelChar("2A19", // standard 16-bit characteristic UUID + BLERead | BLENotify); // remote clients will be able to get notifications if this characteristic changes +BLEStringCharacteristic stringcharacteristic("183E", BLERead | BLEWrite, 31); + + +// Add BLEEncryption tag to require pairing. This controls the LED. +BLEUnsignedCharCharacteristic secretValue("2a3F", BLERead | BLEWrite | BLEEncryption); + +int oldBatteryLevel = 0; // last battery level reading from analog input +long previousMillis = 0; // last time the battery level was checked, in ms + +void setup() { + Serial.begin(9600); // initialize serial communication + while (!Serial); + + pinMode(LED_BUILTIN, OUTPUT); // initialize the built-in LED pin to indicate when a central is connected + + + Serial.println("Serial connected"); + + // IRKs are keys that identify the true owner of a random mac address. + // Add IRKs of devices you are bonded with. + BLE.setGetIRKs([](uint8_t* nIRKs, uint8_t** BADDR_TYPES, uint8_t*** BDAddrs, uint8_t*** IRKs){ + // Set to number of devices + *nIRKs = 2; + + *BDAddrs = new uint8_t*[*nIRKs]; + *IRKs = new uint8_t*[*nIRKs]; + *BADDR_TYPES = new uint8_t[*nIRKs]; + + // Set these to the mac and IRK for your bonded devices as printed in the serial console after bonding. + uint8_t iPhoneMac [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t iPhoneIRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + + uint8_t iPadMac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t iPadIRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, }; + + + (*BADDR_TYPES)[0] = 0; + (*IRKs)[0] = new uint8_t[16]; + memcpy((*IRKs)[0],iPhoneIRK,16); + (*BDAddrs)[0] = new uint8_t[6]; + memcpy((*BDAddrs)[0], iPhoneMac, 6); + + + (*BADDR_TYPES)[1] = 0; + (*IRKs)[1] = new uint8_t[16]; + memcpy((*IRKs)[1],iPadIRK,16); + (*BDAddrs)[1] = new uint8_t[6]; + memcpy((*BDAddrs)[1], iPadMac, 6); + + + return 1; + }); + // The LTK is the secret key which is used to encrypt bluetooth traffic + BLE.setGetLTK([](uint8_t* address, uint8_t* LTK){ + // address is input + Serial.print("Recieved request for address: "); + btct.printBytes(address,6); + + // Set these to the MAC and LTK of your devices after bonding. + uint8_t iPhoneMac [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t iPhoneLTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t iPadMac [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t iPadLTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + + + if(memcmp(iPhoneMac, address, 6)==0){ + memcpy(LTK, iPhoneLTK, 16); + return 1; + }else if(memcmp(iPadMac, address, 6)==0){ + memcpy(LTK, iPadLTK, 16); + } + return 0; + }); + BLE.setStoreIRK([](uint8_t* address, uint8_t* IRK){ + Serial.print(F("New device with MAC : ")); + btct.printBytes(address,6); + Serial.print(F("Need to store IRK : ")); + btct.printBytes(IRK,16); + return 1; + }); + BLE.setStoreLTK([](uint8_t* address, uint8_t* LTK){ + Serial.print(F("New device with MAC : ")); + btct.printBytes(address,6); + Serial.print(F("Need to store LTK : ")); + btct.printBytes(LTK,16); + return 1; + }); + + while(1){// begin initialization + if (!BLE.begin()) { + Serial.println("starting BLE failed!"); + delay(200); + continue; + } + Serial.println("BT init"); + delay(200); + + /* Set a local name for the BLE device + This name will appear in advertising packets + and can be used by remote devices to identify this BLE device + The name can be changed but maybe be truncated based on space left in advertisement packet + */ + + BLE.setDeviceName("Arduino"); + BLE.setLocalName("BatteryMonitor"); + + BLE.setAdvertisedService(batteryService); // add the service UUID + batteryService.addCharacteristic(batteryLevelChar); // add the battery level characteristic + batteryService.addCharacteristic(stringcharacteristic); + batteryService.addCharacteristic(secretValue); + + BLE.addService(batteryService); // Add the battery service + batteryLevelChar.writeValue(oldBatteryLevel); // set initial value for this characteristic + char* stringCharValue = new char[32]; + stringCharValue = "string"; + stringcharacteristic.writeValue(stringCharValue); + secretValue.writeValue(0); + + delay(1000); + + /* Start advertising BLE. It will start continuously transmitting BLE + advertising packets and will be visible to remote BLE central devices + until it receives a new connection */ + + // start advertising + if(!BLE.advertise()){ + Serial.println("failed to advertise bluetooth."); + BLE.stopAdvertise(); + delay(500); + }else{ + Serial.println("advertising..."); + break; + } + BLE.end(); + delay(100); + } +} + + +void loop() { + // wait for a BLE central + BLEDevice central = BLE.central(); + + // if a central is connected to the peripheral: + if (central) { + Serial.print("Connected to central: "); + // print the central's BT address: + Serial.println(central.address()); + + // check the battery level every 200ms + // while the central is connected: + while (central.connected()) { + long currentMillis = millis(); + // if 200ms have passed, check the battery level: + if (currentMillis - previousMillis >= 1000) { + previousMillis = currentMillis; + updateBatteryLevel(); + if(secretValue.value()>0){ + digitalWrite(13,HIGH); + }else{ + digitalWrite(13,LOW); + } + } + } + Serial.print("Disconnected from central: "); + Serial.println(central.address()); + } +} + +void updateBatteryLevel() { + /* Read the current voltage level on the A0 analog input pin. + This is used here to simulate the charge level of a battery. + */ + int battery = analogRead(A0); + int batteryLevel = map(battery, 0, 1023, 0, 100); + + if (batteryLevel != oldBatteryLevel) { // if the battery level has changed + // Serial.print("Battery Level % is now: "); // print it + // Serial.println(batteryLevel); + batteryLevelChar.writeValue(batteryLevel); // and update the battery level characteristic + oldBatteryLevel = batteryLevel; // save the level for next comparison + } +} diff --git a/src/BLECharacteristic.cpp b/src/BLECharacteristic.cpp index 9a07cb9d..1cfbf489 100644 --- a/src/BLECharacteristic.cpp +++ b/src/BLECharacteristic.cpp @@ -47,13 +47,13 @@ BLECharacteristic::BLECharacteristic(BLERemoteCharacteristic* remote) : } } -BLECharacteristic::BLECharacteristic(const char* uuid, uint8_t properties, int valueSize, bool fixedLength) : - BLECharacteristic(new BLELocalCharacteristic(uuid, properties, valueSize, fixedLength)) +BLECharacteristic::BLECharacteristic(const char* uuid, uint16_t permissions, int valueSize, bool fixedLength) : + BLECharacteristic(new BLELocalCharacteristic(uuid, permissions, valueSize, fixedLength)) { } -BLECharacteristic::BLECharacteristic(const char* uuid, uint8_t properties, const char* value) : - BLECharacteristic(new BLELocalCharacteristic(uuid, properties, value)) +BLECharacteristic::BLECharacteristic(const char* uuid, uint16_t permissions, const char* value) : + BLECharacteristic(new BLELocalCharacteristic(uuid, permissions, value)) { } diff --git a/src/BLECharacteristic.h b/src/BLECharacteristic.h index 2ac30b05..da9721e0 100644 --- a/src/BLECharacteristic.h +++ b/src/BLECharacteristic.h @@ -45,8 +45,8 @@ class BLERemoteCharacteristic; class BLECharacteristic { public: BLECharacteristic(); - BLECharacteristic(const char* uuid, uint8_t properties, int valueSize, bool fixedLength = false); - BLECharacteristic(const char* uuid, uint8_t properties, const char* value); + BLECharacteristic(const char* uuid, uint16_t permissions, int valueSize, bool fixedLength = false); + BLECharacteristic(const char* uuid, uint16_t permissions, const char* value); BLECharacteristic(const BLECharacteristic& other); virtual ~BLECharacteristic(); diff --git a/src/BLEProperty.h b/src/BLEProperty.h index aeee4c85..434bd2aa 100644 --- a/src/BLEProperty.h +++ b/src/BLEProperty.h @@ -29,10 +29,19 @@ enum BLEProperty { BLEWrite = 0x08, BLENotify = 0x10, BLEIndicate = 0x20, - BLEAuth = 1 << 6, + BLEAuthSignedWrite = 1 << 6, BLEExtProp = 1 << 7, }; +enum BLEPermission { + BLEEncryption = 1 << 9, + BLEAuthentication = 1 << 10, + BLEAuthorization = 1 << 11, + // BLEWriteEncryption = 1 << 11, + // BLEWriteAuthentication = 1 << 12, + // BLEWriteAuthorization = 1 << 13, +}; + #define ESP_GATT_CHAR_PROP_BIT_BROADCAST (1 << 0) /* 0x01 */ /* relate to BTA_GATT_CHAR_PROP_BIT_BROADCAST in bta/bta_gatt_api.h */ #define ESP_GATT_CHAR_PROP_BIT_READ (1 << 1) /* 0x02 */ /* relate to BTA_GATT_CHAR_PROP_BIT_READ in bta/bta_gatt_api.h */ #define ESP_GATT_CHAR_PROP_BIT_WRITE_NR (1 << 2) /* 0x04 */ /* relate to BTA_GATT_CHAR_PROP_BIT_WRITE_NR in bta/bta_gatt_api.h */ diff --git a/src/BLETypedCharacteristic.h b/src/BLETypedCharacteristic.h index d6e6e4a1..7777d360 100644 --- a/src/BLETypedCharacteristic.h +++ b/src/BLETypedCharacteristic.h @@ -25,7 +25,7 @@ template class BLETypedCharacteristic : public BLECharacteristic { public: - BLETypedCharacteristic(const char* uuid, unsigned char properties); + BLETypedCharacteristic(const char* uuid, unsigned int permissions); int writeValue(T value); int setValue(T value) { return writeValue(value); } @@ -43,8 +43,8 @@ template class BLETypedCharacteristic : public BLECharacteristic T byteSwap(T value); }; -template BLETypedCharacteristic::BLETypedCharacteristic(const char* uuid, unsigned char properties) : - BLECharacteristic(uuid, properties, sizeof(T), true) +template BLETypedCharacteristic::BLETypedCharacteristic(const char* uuid, unsigned int permissions) : + BLECharacteristic(uuid, permissions, sizeof(T), true) { T value; memset(&value, 0x00, sizeof(value)); diff --git a/src/BLETypedCharacteristics.cpp b/src/BLETypedCharacteristics.cpp index 976f6159..800574eb 100644 --- a/src/BLETypedCharacteristics.cpp +++ b/src/BLETypedCharacteristics.cpp @@ -21,72 +21,72 @@ #include "BLETypedCharacteristics.h" -BLEBoolCharacteristic::BLEBoolCharacteristic(const char* uuid, unsigned char properties) : +BLEBoolCharacteristic::BLEBoolCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEBooleanCharacteristic::BLEBooleanCharacteristic(const char* uuid, unsigned char properties) : +BLEBooleanCharacteristic::BLEBooleanCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLECharCharacteristic::BLECharCharacteristic(const char* uuid, unsigned char properties) : +BLECharCharacteristic::BLECharCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEUnsignedCharCharacteristic::BLEUnsignedCharCharacteristic(const char* uuid, unsigned char properties) : +BLEUnsignedCharCharacteristic::BLEUnsignedCharCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEByteCharacteristic::BLEByteCharacteristic(const char* uuid, unsigned char properties) : +BLEByteCharacteristic::BLEByteCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEShortCharacteristic::BLEShortCharacteristic(const char* uuid, unsigned char properties) : +BLEShortCharacteristic::BLEShortCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEUnsignedShortCharacteristic::BLEUnsignedShortCharacteristic(const char* uuid, unsigned char properties) : +BLEUnsignedShortCharacteristic::BLEUnsignedShortCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEWordCharacteristic::BLEWordCharacteristic(const char* uuid, unsigned char properties) : +BLEWordCharacteristic::BLEWordCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEIntCharacteristic::BLEIntCharacteristic(const char* uuid, unsigned char properties) : +BLEIntCharacteristic::BLEIntCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEUnsignedIntCharacteristic::BLEUnsignedIntCharacteristic(const char* uuid, unsigned char properties) : +BLEUnsignedIntCharacteristic::BLEUnsignedIntCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLELongCharacteristic::BLELongCharacteristic(const char* uuid, unsigned char properties) : +BLELongCharacteristic::BLELongCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEUnsignedLongCharacteristic::BLEUnsignedLongCharacteristic(const char* uuid, unsigned char properties) : +BLEUnsignedLongCharacteristic::BLEUnsignedLongCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEFloatCharacteristic::BLEFloatCharacteristic(const char* uuid, unsigned char properties) : +BLEFloatCharacteristic::BLEFloatCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } -BLEDoubleCharacteristic::BLEDoubleCharacteristic(const char* uuid, unsigned char properties) : +BLEDoubleCharacteristic::BLEDoubleCharacteristic(const char* uuid, unsigned int properties) : BLETypedCharacteristic(uuid, properties) { } diff --git a/src/BLETypedCharacteristics.h b/src/BLETypedCharacteristics.h index 465fc046..df8482cf 100644 --- a/src/BLETypedCharacteristics.h +++ b/src/BLETypedCharacteristics.h @@ -24,72 +24,72 @@ class BLEBoolCharacteristic : public BLETypedCharacteristic { public: - BLEBoolCharacteristic(const char* uuid, unsigned char properties); + BLEBoolCharacteristic(const char* uuid, unsigned int permissions); }; class BLEBooleanCharacteristic : public BLETypedCharacteristic { public: - BLEBooleanCharacteristic(const char* uuid, unsigned char properties); + BLEBooleanCharacteristic(const char* uuid, unsigned int permissions); }; class BLECharCharacteristic : public BLETypedCharacteristic { public: - BLECharCharacteristic(const char* uuid, unsigned char properties); + BLECharCharacteristic(const char* uuid, unsigned int permissions); }; class BLEUnsignedCharCharacteristic : public BLETypedCharacteristic { public: - BLEUnsignedCharCharacteristic(const char* uuid, unsigned char properties); + BLEUnsignedCharCharacteristic(const char* uuid, unsigned int permissions); }; class BLEByteCharacteristic : public BLETypedCharacteristic { public: - BLEByteCharacteristic(const char* uuid, unsigned char properties); + BLEByteCharacteristic(const char* uuid, unsigned int permissions); }; class BLEShortCharacteristic : public BLETypedCharacteristic { public: - BLEShortCharacteristic(const char* uuid, unsigned char properties); + BLEShortCharacteristic(const char* uuid, unsigned int permissions); }; class BLEUnsignedShortCharacteristic : public BLETypedCharacteristic { public: - BLEUnsignedShortCharacteristic(const char* uuid, unsigned char properties); + BLEUnsignedShortCharacteristic(const char* uuid, unsigned int permissions); }; class BLEWordCharacteristic : public BLETypedCharacteristic { public: - BLEWordCharacteristic(const char* uuid, unsigned char properties); + BLEWordCharacteristic(const char* uuid, unsigned int permissions); }; class BLEIntCharacteristic : public BLETypedCharacteristic { public: - BLEIntCharacteristic(const char* uuid, unsigned char properties); + BLEIntCharacteristic(const char* uuid, unsigned int permissions); }; class BLEUnsignedIntCharacteristic : public BLETypedCharacteristic { public: - BLEUnsignedIntCharacteristic(const char* uuid, unsigned char properties); + BLEUnsignedIntCharacteristic(const char* uuid, unsigned int permissions); }; class BLELongCharacteristic : public BLETypedCharacteristic { public: - BLELongCharacteristic(const char* uuid, unsigned char properties); + BLELongCharacteristic(const char* uuid, unsigned int permissions); }; class BLEUnsignedLongCharacteristic : public BLETypedCharacteristic { public: - BLEUnsignedLongCharacteristic(const char* uuid, unsigned char properties); + BLEUnsignedLongCharacteristic(const char* uuid, unsigned int permissions); }; class BLEFloatCharacteristic : public BLETypedCharacteristic { public: - BLEFloatCharacteristic(const char* uuid, unsigned char properties); + BLEFloatCharacteristic(const char* uuid, unsigned int permissions); }; class BLEDoubleCharacteristic : public BLETypedCharacteristic { public: - BLEDoubleCharacteristic(const char* uuid, unsigned char properties); + BLEDoubleCharacteristic(const char* uuid, unsigned int permissions); }; #endif diff --git a/src/local/BLELocalCharacteristic.cpp b/src/local/BLELocalCharacteristic.cpp index 333d00b2..2cd801b2 100644 --- a/src/local/BLELocalCharacteristic.cpp +++ b/src/local/BLELocalCharacteristic.cpp @@ -29,20 +29,21 @@ #include "BLELocalCharacteristic.h" -BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint8_t properties, int valueSize, bool fixedLength) : +BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint16_t permissions, int valueSize, bool fixedLength) : BLELocalAttribute(uuid), - _properties(properties), + _properties((uint8_t)(permissions&0x000FF)), _valueSize(min(valueSize, 512)), _valueLength(0), _fixedLength(fixedLength), _handle(0x0000), _broadcast(false), _written(false), - _cccdValue(0x0000) + _cccdValue(0x0000), + _permissions((uint8_t)((permissions&0xFF00)>>8)) { memset(_eventHandlers, 0x00, sizeof(_eventHandlers)); - if (properties & (BLENotify | BLEIndicate)) { + if (permissions & (BLENotify | BLEIndicate)) { BLELocalDescriptor* cccd = new BLELocalDescriptor("2902", (uint8_t*)&_cccdValue, sizeof(_cccdValue)); _descriptors.add(cccd); @@ -51,12 +52,11 @@ BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint8_t propert _value = (uint8_t*)malloc(valueSize); } -BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint8_t properties, const char* value) : - BLELocalCharacteristic(uuid, properties, strlen(value)) +BLELocalCharacteristic::BLELocalCharacteristic(const char* uuid, uint16_t permissions, const char* value) : + BLELocalCharacteristic(uuid, permissions, strlen(value)) { writeValue(value); } - BLELocalCharacteristic::~BLELocalCharacteristic() { for (unsigned int i = 0; i < descriptorCount(); i++) { @@ -84,6 +84,10 @@ uint8_t BLELocalCharacteristic::properties() const return _properties; } +uint8_t BLELocalCharacteristic::permissions() const { + return _permissions; +} + int BLELocalCharacteristic::valueSize() const { return _valueSize; diff --git a/src/local/BLELocalCharacteristic.h b/src/local/BLELocalCharacteristic.h index ee42390a..331cdd5c 100644 --- a/src/local/BLELocalCharacteristic.h +++ b/src/local/BLELocalCharacteristic.h @@ -33,13 +33,14 @@ class BLELocalDescriptor; class BLELocalCharacteristic : public BLELocalAttribute { public: - BLELocalCharacteristic(const char* uuid, uint8_t properties, int valueSize, bool fixedLength = false); - BLELocalCharacteristic(const char* uuid, uint8_t properties, const char* value); + BLELocalCharacteristic(const char* uuid, uint16_t permissions, int valueSize, bool fixedLength = false); + BLELocalCharacteristic(const char* uuid, uint16_t permissions, const char* value); virtual ~BLELocalCharacteristic(); virtual enum BLEAttributeType type() const; uint8_t properties() const; + uint8_t permissions() const; int valueSize() const; const uint8_t* value() const; @@ -75,6 +76,7 @@ class BLELocalCharacteristic : public BLELocalAttribute { private: uint8_t _properties; + uint8_t _permissions; int _valueSize; uint8_t* _value; uint16_t _valueLength; diff --git a/src/local/BLELocalDevice.cpp b/src/local/BLELocalDevice.cpp index df11e971..49cc6603 100644 --- a/src/local/BLELocalDevice.cpp +++ b/src/local/BLELocalDevice.cpp @@ -108,7 +108,7 @@ int BLELocalDevice::begin() end(); return 0; } - if (HCI.setLeEventMask(0x00000000000001FF) != 0) { + if (HCI.setLeEventMask(0x00000000000003FF) != 0) { end(); return 0; } @@ -121,6 +121,59 @@ int BLELocalDevice::begin() return 0; } + /// The HCI should allow automatic address resolution. + + // // If we have callbacks to rememember bonded devices: + // if(HCI._getIRKs!=0){ + // uint8_t nIRKs = 0; + // uint8_t** BADDR_Type = new uint8_t*; + // uint8_t*** BADDRs = new uint8_t**; + // uint8_t*** IRKs = new uint8_t**; + // uint8_t* memcheck; + + + // if(!HCI._getIRKs(&nIRKs, BADDR_Type, BADDRs, IRKs)){ + // Serial.println("error"); + // } + // for(int i=0; i>8)), _valueHandle(valueHandle), _value(NULL), _valueLength(0), diff --git a/src/remote/BLERemoteCharacteristic.h b/src/remote/BLERemoteCharacteristic.h index d0ac09bc..b53ab031 100644 --- a/src/remote/BLERemoteCharacteristic.h +++ b/src/remote/BLERemoteCharacteristic.h @@ -29,10 +29,11 @@ class BLERemoteCharacteristic : public BLERemoteAttribute { public: - BLERemoteCharacteristic(const uint8_t uuid[], uint8_t uuidLen, uint16_t connectionHandle, uint16_t startHandle, uint8_t properties, uint16_t valueHandle); + BLERemoteCharacteristic(const uint8_t uuid[], uint8_t uuidLen, uint16_t connectionHandle, uint16_t startHandle, uint16_t permissions, uint16_t valueHandle); virtual ~BLERemoteCharacteristic(); uint8_t properties() const; + uint8_t permissions() const; const uint8_t* value() const; int valueLength() const; @@ -66,6 +67,7 @@ class BLERemoteCharacteristic : public BLERemoteAttribute { uint16_t _connectionHandle; uint16_t _startHandle; uint8_t _properties; + uint8_t _permissions; uint16_t _valueHandle; uint8_t* _value; diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index 5bd5e2aa..6ef6bc31 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -253,6 +253,15 @@ void ATTClass::addConnection(uint16_t handle, uint8_t role, uint8_t peerBdaddrTy _peers[peerIndex].mtu = 23; _peers[peerIndex].addressType = peerBdaddrType; memcpy(_peers[peerIndex].address, peerBdaddr, sizeof(_peers[peerIndex].address)); + uint8_t BDADDr[6]; + for(int i=0; i<6; i++) BDADDr[5-i] = peerBdaddr[i]; + if(HCI.tryResolveAddress(BDADDr,_peers[peerIndex].resolvedAddress)){ +#ifdef _BLE_TRACE_ + Serial.println("Found match."); +#endif + }else{ + memset(_peers[peerIndex].resolvedAddress, 0, 6); + } if (_eventHandlers[BLEConnected]) { _eventHandlers[BLEConnected](BLEDevice(peerBdaddrType, peerBdaddr)); @@ -515,6 +524,7 @@ bool ATTClass::disconnect() _peers[i].role = 0x00; _peers[i].addressType = 0x00; memset(_peers[i].address, 0x00, sizeof(_peers[i].address)); + memset(_peers[i].resolvedAddress, 0x00, sizeof(_peers[i].resolvedAddress)); _peers[i].mtu = 23; if (_peers[i].device) { @@ -1007,7 +1017,8 @@ void ATTClass::readOrReadBlobReq(uint16_t connectionHandle, uint16_t mtu, uint8_ return; } // If characteristic requires encryption send error & hold response until encrypted - if ((characteristic->properties() & BLEAuth) > 0 && (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES)==0) { + if ((characteristic->permissions() & (BLEPermission::BLEEncryption >> 8)) > 0 && + (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES)==0 ) { holdResponse = true; sendError(connectionHandle, opcode, handle, ATT_ECODE_INSUFF_ENC); } @@ -1231,7 +1242,8 @@ void ATTClass::writeReqOrCmd(uint16_t connectionHandle, uint16_t mtu, uint8_t op return; } // Check permssion - if((characteristic->properties() & BLEProperty::BLEAuth)> 0 && (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES) == 0){ + if((characteristic->permissions() &( BLEPermission::BLEEncryption >> 8)) > 0 && + (getPeerEncryption(connectionHandle) & PEER_ENCRYPTION::ENCRYPTED_AES) == 0){ holdResponse = true; sendError(connectionHandle, ATT_OP_WRITE_REQ, handle, ATT_ECODE_INSUFF_ENC); } @@ -1817,7 +1829,7 @@ int ATTClass::setPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[3]){ // Return the connection handle for the first peer that is requesting encryption uint16_t ATTClass::getPeerEncrptingConnectionHandle(){ for(int i=0; i 0){ + if((_peers[i].encryption & PEER_ENCRYPTION::REQUESTED_ENCRYPTION) > 0){ return _peers[i].connectionHandle; } } @@ -1861,7 +1873,7 @@ int ATTClass::getPeerAddrWithType(uint16_t connectionHandle, uint8_t peerAddr[]) peerAddr[6-k] = _peers[i].address[k]; } if(_peers[i].addressType){ - peerAddr[0] = 0x01; + peerAddr[0] = _peers[i].addressType; }else{ peerAddr[0] = 0x00; } @@ -1869,6 +1881,31 @@ int ATTClass::getPeerAddrWithType(uint16_t connectionHandle, uint8_t peerAddr[]) } return 0; } +// Get the resolved address for a peer if it exists +int ATTClass::getPeerResolvedAddress(uint16_t connectionHandle, uint8_t resolvedAddress[]){ + for(int i=0; i #include "BLEDevice.h" +#include "keyDistribution.h" #define ATT_CID 0x0004 #define BLE_CTL 0x0008 @@ -37,11 +38,12 @@ enum PEER_ENCRYPTION { NO_ENCRYPTION = 0, - REQUESTED_ENCRYPTION = 1 << 0, - SENT_PUBKEY = 1 << 1, - DH_KEY_CALULATED = 1 << 2, - RECEIVED_DH_CHECK = 1 << 3, - SENT_DH_CHECK = 1 << 4, + PAIRING_REQUEST = 1 << 0, + REQUESTED_ENCRYPTION = 1 << 1, + SENT_PUBKEY = 1 << 2, + DH_KEY_CALULATED = 1 << 3, + RECEIVED_DH_CHECK = 1 << 4, + SENT_DH_CHECK = 1 << 5, ENCRYPTED_AES = 1 << 7 }; @@ -94,11 +96,17 @@ class ATTClass { virtual int getPeerAddrWithType(uint16_t connectionHandle, uint8_t peerAddr[]); virtual int setPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[]); virtual int getPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[]); + virtual int getPeerResolvedAddress(uint16_t connectionHandle, uint8_t* resolvedAddress); uint8_t holdBuffer[64]; uint8_t writeBuffer[64]; uint8_t holdBufferSize; uint8_t writeBufferSize; virtual int processWriteBuffer(); + KeyDistribution remoteKeyDistribution; + KeyDistribution localKeyDistribution; + uint8_t peerIRK[16]; + /// This is just a random number... Not sure it has use unless privacy mode is active. + uint8_t localIRK[16] = {0x54,0x83,0x63,0x7c,0xc5,0x1e,0xf7,0xec,0x32,0xdd,0xad,0x51,0x89,0x4b,0x9e,0x07}; private: virtual void error(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); virtual void mtuReq(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); @@ -139,6 +147,7 @@ class ATTClass { uint8_t role; uint8_t addressType; uint8_t address[6]; + uint8_t resolvedAddress[6]; uint16_t mtu; BLERemoteDevice* device; uint8_t encryption; diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 805a2c3d..c3d66848 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -34,6 +34,7 @@ #define EVT_CMD_COMPLETE 0x0e #define EVT_CMD_STATUS 0x0f #define EVT_NUM_COMP_PKTS 0x13 +#define EVT_RETURN_LINK_KEYS 0x15 #define EVT_UNKNOWN 0x10 #define EVT_LE_META_EVENT 0x3e @@ -449,6 +450,161 @@ int HCIClass::leConnUpdate(uint16_t handle, uint16_t minInterval, uint16_t maxIn return sendCommand(OGF_LE_CTL << 10 | OCF_LE_CONN_UPDATE, sizeof(leConnUpdateData), &leConnUpdateData); } +int HCIClass::saveNewAddress(uint8_t addressType, uint8_t* address, uint8_t* peerIrk, uint8_t* localIrk){ + if(_storeIRK!=0){ + _storeIRK(address, peerIrk); + } + // Again... this should work + // leAddResolvingAddress(addressType, address, peerIrk, localIrk); +} +int HCIClass::leAddResolvingAddress(uint8_t addressType, uint8_t* peerAddress, uint8_t* peerIrk, uint8_t* localIrk){ + leStopResolvingAddresses(); + + struct __attribute__ ((packed)) AddDevice { + uint8_t peerAddressType; + uint8_t peerAddress[6]; + uint8_t peerIRK[16]; + uint8_t localIRK[16]; + } addDevice; + addDevice.peerAddressType = addressType; + for(int i=0; i<6; i++) addDevice.peerAddress[5-i] = peerAddress[i]; + for(int i=0; i<16; i++) { + addDevice.peerIRK[15-i] = peerIrk[i]; + addDevice.localIRK[15-i] = localIrk[i]; + } + Serial.print("ADDTYPE :"); + btct.printBytes(&addDevice.peerAddressType,1); + Serial.print("adddddd :"); + btct.printBytes(addDevice.peerAddress,6); + Serial.print("Peer IRK :"); + btct.printBytes(addDevice.peerIRK,16); + Serial.print("localIRK :"); + btct.printBytes(addDevice.localIRK,16); + sendCommand(OGF_LE_CTL << 10 | 0x27, sizeof(addDevice), &addDevice); + + leStartResolvingAddresses(); +} +int HCIClass::leStopResolvingAddresses(){ + uint8_t enable = 0; + return HCI.sendCommand(OGF_LE_CTL << 10 | 0x2D, 1,&enable); // Disable address resolution +} +int HCIClass::leStartResolvingAddresses(){ + uint8_t enable = 1; + return HCI.sendCommand(OGF_LE_CTL << 10 | 0x2D, 1,&enable); // Disable address resolution +} +int HCIClass::leReadPeerResolvableAddress(uint8_t peerAddressType, uint8_t* peerIdentityAddress, uint8_t* peerResolvableAddress){ + struct __attribute__ ((packed)) Request { + uint8_t addressType; + uint8_t identityAddress[6]; + } request; + request.addressType = peerAddressType; + for(int i=0; i<6; i++) request.identityAddress[5-i] = peerIdentityAddress[i]; + + + int res = sendCommand(OGF_LE_CTL << 10 | 0x2B, sizeof(request), &request); + Serial.print("res: 0x"); + Serial.println(res, HEX); + if(res==0){ + struct __attribute__ ((packed)) Response { + uint8_t status; + uint8_t peerResolvableAddress[6]; + } *response = (Response*)_cmdResponse; + Serial.print("Address resolution status: 0x"); + Serial.println(response->status, HEX); + Serial.print("peer resolvable address: "); + btct.printBytes(response->peerResolvableAddress,6); + } + return res; +} + +int HCIClass::writeLK(uint8_t peerAddress[], uint8_t LK[]){ + struct __attribute__ ((packed)) StoreLK { + uint8_t nKeys; + uint8_t BD_ADDR[6]; + uint8_t LTK[16]; + } storeLK; + storeLK.nKeys = 1; + memcpy(storeLK.BD_ADDR, peerAddress, 6); + for(int i=0; i<16; i++) storeLK.LTK[15-i] = LK[i]; + HCI.sendCommand(OGF_HOST_CTL << 10 | 0x11, sizeof(storeLK), &storeLK); +} +int HCIClass::readStoredLKs(){ + uint8_t BD_ADDR[6]; + readStoredLK(BD_ADDR, 1); +} +int HCIClass::readStoredLK(uint8_t BD_ADDR[], uint8_t read_all ){ + struct __attribute__ ((packed)) Request { + uint8_t BD_ADDR[6]; + uint8_t read_a; + } request = {0,0}; + for(int i=0; i<6; i++) request.BD_ADDR[5-i] = BD_ADDR[i]; + request.read_a = read_all; + return sendCommand(OGF_HOST_CTL << 10 | 0xD, sizeof(request), &request); +} + +int HCIClass::tryResolveAddress(uint8_t* BDAddr, uint8_t* address){ + uint8_t iphone[16] = {0xA6, 0xD2, 0xD, 0xD3, 0x4F, 0x13, 0x42, 0x4F, 0xE1, 0xC1, 0xFD, 0x22, 0x2E, 0xC5, 0x6A, 0x2D}; + uint8_t irk[16]; + for(int i=0; i<16; i++) irk[15-i] = iphone[i]; + bool foundMatch = false; + if(HCI._getIRKs!=0){ + uint8_t nIRKs = 0; + uint8_t** BDAddrType = new uint8_t*; + uint8_t*** BADDRs = new uint8_t**; + uint8_t*** IRKs = new uint8_t**; + uint8_t* memcheck; + + + if(!HCI._getIRKs(&nIRKs, BDAddrType, BADDRs, IRKs)){ + Serial.println("error getting IRKs."); + } + for(int i=0; ienabled,1); #endif if(encryptionChange->enabled>0){ + // 0001 1110 + if((ATT.getPeerEncryption(encryptionChange->connectionHandle)&PEER_ENCRYPTION::PAIRING_REQUEST)>0){ + if(ATT.localKeyDistribution.EncKey()){ +#ifdef _BLE_TRACE_ + Serial.println("Enc key set but sould be ignored"); +#endif + }else{ +#ifdef _BLE_TRACE_ + Serial.println("No enc key distribution"); +#endif + } + // From page 1681 bluetooth standard - order matters + if(ATT.localKeyDistribution.IdKey()){ + /// We shall distribute IRK and address using identity information + { + uint8_t response[17]; + response[0] = CONNECTION_IDENTITY_INFORMATION; // Identity information. + for(int i=0; i<16; i++) response[16-i] = ATT.localIRK[i]; + HCI.sendAclPkt(encryptionChange->connectionHandle, SECURITY_CID, sizeof(response), response); +#ifdef _BLE_TRACE_ + Serial.println("Distribute ID Key"); +#endif + } + { + uint8_t response[8]; + response[0] = CONNECTION_IDENTITY_ADDRESS; // Identity address information + response[1] = 0x00; // Static local address + for(int i=0; i<6; i++) response[7-i] = HCI.localAddr[i]; + HCI.sendAclPkt(encryptionChange->connectionHandle, SECURITY_CID, sizeof(response), response); + } + } + if(ATT.localKeyDistribution.SignKey()){ + /// We shall distribut CSRK +#ifdef _BLE_TRACE_ + Serial.println("We shall distribute CSRK // not implemented"); +#endif + + }else{ + // Serial.println("We don't want to distribute CSRK"); + } + if(ATT.localKeyDistribution.LinkKey()){ +#ifdef _BLE_TRACE_ + Serial.println("We would like to use LTK to generate BR/EDR // not implemented"); +#endif + } + }else{ +#ifdef _BLE_TRACE_ + Serial.println("Reconnection, not pairing so no keys"); + Serial.println(ATT.getPeerEncryption(encryptionChange->connectionHandle),HEX); +#endif + } + ATT.setPeerEncryption(encryptionChange->connectionHandle, PEER_ENCRYPTION::ENCRYPTED_AES); if(ATT.writeBufferSize > 0){ ATT.processWriteBuffer(); @@ -740,6 +948,27 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) data += 2; } } + else if(eventHdr->evt == EVT_RETURN_LINK_KEYS) + { + uint8_t num_keys = (uint8_t)pdata[sizeof(HCIEventHdr)]; + // Serial.print("N keys: "); + // Serial.println(num_keys); + uint8_t BD_ADDRs[num_keys][6]; + uint8_t LKs[num_keys][16]; + auto nAddresss = [pdata](uint8_t nAddr)->uint8_t*{ + return (uint8_t*) &pdata[sizeof(HCIEventHdr)] + 1 + nAddr*6 + nAddr*16; + }; + auto nLK = [pdata](uint8_t nLK)->uint8_t*{ + return (uint8_t*) &pdata[sizeof(HCIEventHdr)] + 1 + (nLK+1)*6 + nLK*16; + }; + // Serial.println("Stored LKs are: "); + // for(int i=0; ievt == 0x10) { struct __attribute__ ((packed)) CmdHardwareError { @@ -761,6 +990,57 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) Serial.println(leMetaHeader->subevent,HEX); #endif switch((LE_META_EVENT)leMetaHeader->subevent){ + case 0x0A:{ + struct __attribute__ ((packed)) EvtLeConnectionComplete { + uint8_t status; + uint16_t handle; + uint8_t role; + uint8_t peerBdaddrType; + uint8_t peerBdaddr[6]; + uint8_t localResolvablePrivateAddress[6]; + uint8_t peerResolvablePrivateAddress[6]; + uint16_t interval; + uint16_t latency; + uint16_t supervisionTimeout; + uint8_t masterClockAccuracy; + } *leConnectionComplete = (EvtLeConnectionComplete*)&pdata[sizeof(HCIEventHdr) + sizeof(LeMetaEventHeader)]; + + if (leConnectionComplete->status == 0x00) { + ATT.addConnection(leConnectionComplete->handle, + leConnectionComplete->role, + leConnectionComplete->peerBdaddrType, + leConnectionComplete->peerBdaddr, + leConnectionComplete->interval, + leConnectionComplete->latency, + leConnectionComplete->supervisionTimeout, + leConnectionComplete->masterClockAccuracy); + + L2CAPSignaling.addConnection(leConnectionComplete->handle, + leConnectionComplete->role, + leConnectionComplete->peerBdaddrType, + leConnectionComplete->peerBdaddr, + leConnectionComplete->interval, + leConnectionComplete->latency, + leConnectionComplete->supervisionTimeout, + leConnectionComplete->masterClockAccuracy); + } + // uint8_t address[6]; + // uint8_t BDAddr[6]; + // for(int i=0; i<6; i++) BDAddr[5-i] = leConnectionComplete->peerBdaddr[i]; + // leReadPeerResolvableAddress(leConnectionComplete->peerBdaddrType,BDAddr,address); + // Serial.print("Resolving address: "); + // btct.printBytes(BDAddr, 6); + // Serial.print("BT answer : "); + // btct.printBytes(address, 6); + +#ifdef _BLE_TRACE_ + Serial.print("Resolved peer : "); + btct.printBytes(leConnectionComplete->peerResolvablePrivateAddress,6); + Serial.print("Resolved local : "); + btct.printBytes(leConnectionComplete->localResolvablePrivateAddress,6); +#endif + break; + } case CONN_COMPLETE:{ struct __attribute__ ((packed)) EvtLeConnectionComplete { uint8_t status; @@ -793,6 +1073,14 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) leConnectionComplete->supervisionTimeout, leConnectionComplete->masterClockAccuracy); } + uint8_t address[6]; + uint8_t BDAddr[6]; + for(int i=0; i<6; i++) BDAddr[5-i] = leConnectionComplete->peerBdaddr[i]; + // leReadPeerResolvableAddress(leConnectionComplete->peerBdaddrType,BDAddr,address); + // Serial.print("Resolving address: "); + // btct.printBytes(BDAddr, 6); + // Serial.print("BT answer : "); + // btct.printBytes(address, 6); break; } case ADVERTISING_REPORT:{ @@ -818,8 +1106,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) } break; } - case LONG_TERM_KEY_REQUEST: - { + case LONG_TERM_KEY_REQUEST:{ struct __attribute__ ((packed)) LTKRequest { uint8_t subEventCode; @@ -836,14 +1123,25 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) Serial.print("EDIV : "); btct.printBytes(ltkRequest->encryptedDiversifier,2); #endif - + // Load our LTK for this connection. + uint8_t peerAddr[7]; + uint8_t resolvableAddr[6]; + ATT.getPeerAddrWithType(ltkRequest->connectionHandle, peerAddr); + + if(ATT.getPeerResolvedAddress(ltkRequest->connectionHandle, resolvableAddr) + && !((ATT.getPeerEncryption(ltkRequest->connectionHandle) & PEER_ENCRYPTION::PAIRING_REQUEST)>0)){ + _getLTK(resolvableAddr, HCI.LTK); + }else{ + _getLTK(&peerAddr[1], HCI.LTK); + } + // } // Send our LTK back struct __attribute__ ((packed)) LTKReply { uint16_t connectionHandle; uint8_t LTK[16]; } ltkReply = {0,0}; - ltkReply.connectionHandle = ATT.getPeerEncrptingConnectionHandle(); + ltkReply.connectionHandle = ltkRequest->connectionHandle; for(int i=0; i<16; i++) ltkReply.LTK[15-i] = HCI.LTK[i]; int result = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::LONG_TERM_KEY_REPLY,sizeof(ltkReply), <kReply); @@ -875,8 +1173,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) } break; } - case REMOTE_CONN_PARAM_REQ: - { + case REMOTE_CONN_PARAM_REQ:{ struct __attribute__ ((packed)) RemoteConnParamReq { uint8_t subEventCode; uint16_t connectionHandle; @@ -914,8 +1211,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) sendCommand(OGF_LE_CTL << 10 | 0x20, sizeof(remoteConnParamReqReply), &remoteConnParamReqReply); break; } - case READ_LOCAL_P256_COMPLETE: - { + case READ_LOCAL_P256_COMPLETE:{ struct __attribute__ ((packed)) EvtReadLocalP256Complete{ uint8_t subEventCode; uint8_t status; @@ -1007,8 +1303,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) } break; } - case GENERATE_DH_KEY_COMPLETE: - { + case GENERATE_DH_KEY_COMPLETE:{ struct __attribute__ ((packed)) EvtLeDHKeyComplete{ uint8_t subEventCode; uint8_t status; diff --git a/src/utility/HCI.h b/src/utility/HCI.h index 0b9c8302..de15bdf8 100644 --- a/src/utility/HCI.h +++ b/src/utility/HCI.h @@ -90,6 +90,17 @@ class HCIClass { virtual int leCancelConn(); virtual int leEncrypt(uint8_t* Key, uint8_t* plaintext, uint8_t* status, uint8_t* ciphertext); + virtual int saveNewAddress(uint8_t addressType, uint8_t* address, uint8_t* peerIrk, uint8_t* remoteIrk); + virtual int leAddResolvingAddress(uint8_t addressType, uint8_t* address, uint8_t* peerIrk, uint8_t* remoteIrk); + virtual int leStopResolvingAddresses(); + virtual int leStartResolvingAddresses(); + virtual int leReadPeerResolvableAddress(uint8_t peerAddressType, uint8_t* peerIdentityAddress, uint8_t* peerResolvableAddress); + + virtual int readStoredLKs(); + virtual int readStoredLK(uint8_t BD_ADDR[], uint8_t read_all = 0); + virtual int writeLK(uint8_t peerAddress[], uint8_t LK[]); + virtual int tryResolveAddress(uint8_t* BDAddr, uint8_t* address); + virtual int sendAclPkt(uint16_t handle, uint8_t cid, uint8_t plen, void* data); virtual int disconnect(uint16_t handle); @@ -105,6 +116,12 @@ class HCIClass { uint8_t DHKey[32]; uint8_t localAddr[6]; uint8_t LTK[16]; + + int (*_storeIRK)(uint8_t* address, uint8_t* peerIrk); + int (*_getIRKs)(uint8_t* nIRKs,uint8_t** BADDR_type, uint8_t*** BADDRs, uint8_t*** IRKs); + int (*_storeLTK)(uint8_t*, uint8_t*); + int (*_getLTK)(uint8_t*, uint8_t*); + private: virtual void handleAclDataPkt(uint8_t plen, uint8_t pdata[]); diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index 3779eb29..512e7665 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -21,6 +21,7 @@ #include "ATT.h" #include "btct.h" #include "L2CAPSignaling.h" +#include "keyDistribution.h" #define CONNECTION_PARAMETER_UPDATE_REQUEST 0x12 #define CONNECTION_PARAMETER_UPDATE_RESPONSE 0x13 @@ -137,13 +138,22 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t initiatorKeyDistribution; uint8_t responderKeyDistribution; } *pairingRequest = (PairingRequest*)l2capSignalingHdr->data; + + + ATT.remoteKeyDistribution = KeyDistribution(pairingRequest->initiatorKeyDistribution); + ATT.localKeyDistribution = KeyDistribution(pairingRequest->responderKeyDistribution); + KeyDistribution rkd(pairingRequest->responderKeyDistribution); uint8_t peerIOCap[3]; peerIOCap[0] = pairingRequest->authReq; peerIOCap[1] = pairingRequest->oobDataFlag; peerIOCap[2] = pairingRequest->ioCapability; ATT.setPeerIOCap(connectionHandle, peerIOCap); - + ATT.setPeerEncryption(connectionHandle, ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::PAIRING_REQUEST); +#ifdef _BLE_TRACE_ + Serial.print("Peer encryption : 0b"); + Serial.print(ATT.getPeerEncryption(connectionHandle), BIN); +#endif struct __attribute__ ((packed)) PairingResponse { uint8_t code; uint8_t ioCapability; @@ -154,7 +164,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t responderKeyDistribution; } response = { CONNECTION_PAIRING_RESPONSE, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 0x10, 0b1011, 0b1011}; - HCI.sendAclPkt(connectionHandle, 0x06, sizeof(response), &response); + HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); } else if (code == CONNECTION_PAIRING_RANDOM) { @@ -173,7 +183,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t } response = { CONNECTION_PAIRING_RANDOM, 0}; for(int i=0; i< 16; i++) response.Nb[15-i] = HCI.Nb[i]; - HCI.sendAclPkt(connectionHandle, 0x06, sizeof(response), &response); + HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); } else if (code == CONNECTION_PAIRING_RESPONSE) { @@ -190,6 +200,31 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t Serial.println(pairingFailed->reason,HEX); #endif } + else if (code == CONNECTION_IDENTITY_INFORMATION){ + struct __attribute__ ((packed)) IdentityInformation { + uint8_t code; + uint8_t PeerIRK[16]; + } *identityInformation = (IdentityInformation*)data; + for(int i=0; i<16; i++) ATT.peerIRK[15-i] = identityInformation->PeerIRK[i]; +#ifdef _BLE_TRACE_ + Serial.println("Saved peer IRK"); +#endif + } + else if (code == CONNECTION_IDENTITY_ADDRESS){ + struct __attribute__ ((packed)) IdentityAddress { + uint8_t code; + uint8_t addressType; + uint8_t address[6]; + } *identityAddress = (IdentityAddress*)data; + // we can save this information now. + uint8_t peerAddress[6]; + for(int i; i<6; i++) peerAddress[5-i] = identityAddress->address[i]; + + HCI.saveNewAddress(identityAddress->addressType, peerAddress, ATT.peerIRK, ATT.localIRK); + if(HCI._storeLTK!=0){ + HCI._storeLTK(peerAddress, HCI.LTK); + } + } else if (code == CONNECTION_PAIRING_PUBLIC_KEY){ /// Received a public key struct __attribute__ ((packed)) ConnectionPairingPublicKey { @@ -211,7 +246,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t LE_COMMAND::READ_LOCAL_P256, }; - if(ATT.setPeerEncryption(connectionHandle,PEER_ENCRYPTION::REQUESTED_ENCRYPTION)){ + if(ATT.setPeerEncryption(connectionHandle, ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::REQUESTED_ENCRYPTION)){ #ifdef _BLE_TRACE_ Serial.println("[Info] Pairing public key"); Serial.println("Requested encryption stored."); @@ -244,7 +279,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t HCI.readBdAddr(); ATT.setPeerEncryption(connectionHandle, encryptionState); - if(encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED > 0){ + if((encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED) > 0){ // We've already calculated the DHKey so we can calculate our check and send it. uint8_t MacKey[16]; diff --git a/src/utility/btct.cpp b/src/utility/btct.cpp index 8fad5173..d78e328e 100644 --- a/src/utility/btct.cpp +++ b/src/utility/btct.cpp @@ -119,6 +119,38 @@ int BluetoothCryptoToolbox::f6(uint8_t W[], uint8_t N1[],uint8_t N2[],uint8_t R[ return 1; } // AES_CMAC from RFC +int BluetoothCryptoToolbox::ah(uint8_t k[16], uint8_t r[3], uint8_t* result) +{ + uint8_t r_[16]; + int i=0; + for(i=0; i<16; i++) r_[i] = 0; + for(i=0; i<3; i++) r_[i+13] = r[i]; + uint8_t intermediate[16]; + AES_128(k,r_,intermediate); + for(i=0; i<3; i++){ + result[i] = intermediate[i+13]; + } + return 1; +} +void BluetoothCryptoToolbox::testAh() +{ + uint8_t irk[16] = {0xec,0x02,0x34,0xa3,0x57,0xc8,0xad,0x05,0x34,0x10,0x10,0xa6,0x0a,0x39,0x7d,0x9b}; + uint8_t r[3] = {0x70,0x81,0x94}; + uint8_t expected_AES[16] = {0x15,0x9d,0x5f,0xb7,0x2e,0xbe,0x23,0x11,0xa4,0x8c,0x1b,0xdc,0xc4,0x0d,0xfb,0xaa}; + uint8_t expected_final[3] = {0x0d,0xfb,0xaa}; + + for(int i=0; i<3; i++) r[2-i] = expected_final[3+i]; + uint8_t ourResult[3]; + ah(irk, expected_final, ourResult); + + + Serial.print("Expected : "); + printBytes(&expected_final[3], 3); + Serial.print("Actual : "); + printBytes(ourResult, 3); +} + + void BluetoothCryptoToolbox::AES_CMAC ( unsigned char *key, unsigned char *input, int length, unsigned char *mac ) { diff --git a/src/utility/btct.h b/src/utility/btct.h index 87e515af..43c0b0d6 100644 --- a/src/utility/btct.h +++ b/src/utility/btct.h @@ -13,9 +13,11 @@ class BluetoothCryptoToolbox{ int f5(uint8_t DHKey[],uint8_t N_master[], uint8_t N_slave[], uint8_t BD_ADDR_master[], uint8_t BD_ADDR_slave[], uint8_t MacKey[], uint8_t LTK[]); int f6(uint8_t W[], uint8_t N1[],uint8_t N2[],uint8_t R[], uint8_t IOCap[], uint8_t A1[], uint8_t A2[], uint8_t Ex[]); + int ah(uint8_t k[16], uint8_t r[3], uint8_t result[3]); void test(); void testF5(); void testF6(); + void testAh(); private: int AES_128(uint8_t key[], uint8_t data_in[], uint8_t data_out[]); void leftshift_onebit(unsigned char *input,unsigned char *output); diff --git a/src/utility/keyDistribution.cpp b/src/utility/keyDistribution.cpp new file mode 100644 index 00000000..1a9fe099 --- /dev/null +++ b/src/utility/keyDistribution.cpp @@ -0,0 +1,24 @@ +#include "keyDistribution.h" + +KeyDistribution::KeyDistribution(){} +KeyDistribution::KeyDistribution(uint8_t octet):_octet(octet){} + +#define ENCKEY 0b00000001 +#define IDKEY 0b00000010 +#define SIGNKEY 0b00000100 +#define LINKKEY 0b00001000 +void KeyDistribution::setOctet( uint8_t octet){_octet = octet;} +uint8_t KeyDistribution::getOctet() {return _octet;} +// Ignored when SMP is on LE transport +bool KeyDistribution::EncKey(){ return (_octet & ENCKEY)>0;} +// Device shall distribute IRK using Identity information command followed by its address using Identity address information +bool KeyDistribution::IdKey(){ return (_octet & IDKEY)>0;} +// Device shall distribute CSRK using signing information command +bool KeyDistribution::SignKey(){ return (_octet & SIGNKEY)>0;} +// Device would like to derive BR/EDR from LTK +bool KeyDistribution::LinkKey(){ return (_octet & LINKKEY)>0;} + +void KeyDistribution::setEncKey(bool state) { _octet= state? _octet|ENCKEY : _octet&~ENCKEY;} +void KeyDistribution::setIdKey(bool state) { _octet= state? _octet|IDKEY : _octet&~IDKEY;} +void KeyDistribution::setSignKey(bool state){ _octet= state? _octet|SIGNKEY : _octet&~SIGNKEY;} +void KeyDistribution::setLinkKey(bool state){ _octet= state? _octet|LINKKEY : _octet&~LINKKEY;} \ No newline at end of file diff --git a/src/utility/keyDistribution.h b/src/utility/keyDistribution.h new file mode 100644 index 00000000..d78fcc1a --- /dev/null +++ b/src/utility/keyDistribution.h @@ -0,0 +1,29 @@ +#ifndef _KEY_DISTRIBUTION_H_ +#define _KEY_DISTRIBUTION_H_ +#include + +class KeyDistribution{ +public: + KeyDistribution(); + KeyDistribution(uint8_t octet); + void setOctet( uint8_t octet); + uint8_t getOctet(); + // Ignored when SMP is on LE transport + bool EncKey(); + // Device shall distribute IRK using Identity information command followed by its address using Identity address information + bool IdKey(); + // Device shall distribute CSRK using signing information command + bool SignKey(); + // Device would like to derive BR/EDR from LTK + bool LinkKey(); + + void setEncKey(bool state); + void setIdKey(bool state); + void setSignKey(bool state); + void setLinkKey(bool state); +private: + uint8_t _octet; + // 1. IRK by the slave2. BD ADDR by the slave3. CSRK by the slave4. IRK by the master5. BD_ADDR by the master6. CSRK by the master +}; + +#endif \ No newline at end of file From 1431f3de4f6a4d733b1438bb3fe72a3ea2b614d9 Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Sat, 2 Jan 2021 23:08:27 +0000 Subject: [PATCH 04/13] Fixed packet fragmentation --- src/utility/HCI.cpp | 21 +++++++++++++---- src/utility/L2CAPSignaling.cpp | 13 +++++++++-- src/utility/bitDescriptions.cpp | 30 ++++++++++++++++++++++++ src/utility/bitDescriptions.h | 41 +++++++++++++++++++++++++++++++++ src/utility/keyDistribution.cpp | 2 +- 5 files changed, 100 insertions(+), 7 deletions(-) create mode 100644 src/utility/bitDescriptions.cpp create mode 100644 src/utility/bitDescriptions.h diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index c3d66848..376cfb9c 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -707,9 +707,7 @@ void HCIClass::handleAclDataPkt(uint8_t /*plen*/, uint8_t pdata[]) uint16_t cid; } *aclHdr = (HCIACLHdr*)pdata; -#ifdef _BLE_TRACE_ - Serial.println("Received data"); -#endif + uint16_t aclFlags = (aclHdr->handle & 0xf000) >> 12; if ((aclHdr->dlen - 4) != aclHdr->len) { @@ -729,6 +727,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; } @@ -751,7 +760,11 @@ void HCIClass::handleAclDataPkt(uint8_t /*plen*/, uint8_t pdata[]) #ifdef _BLE_TRACE_ Serial.println("Security data"); #endif - L2CAPSignaling.handleSecurityData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]); + 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)) { diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index 512e7665..7d9c2c47 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -22,7 +22,7 @@ #include "btct.h" #include "L2CAPSignaling.h" #include "keyDistribution.h" - +#include "bitDescriptions.h" #define CONNECTION_PARAMETER_UPDATE_REQUEST 0x12 #define CONNECTION_PARAMETER_UPDATE_RESPONSE 0x13 @@ -143,6 +143,15 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t ATT.remoteKeyDistribution = KeyDistribution(pairingRequest->initiatorKeyDistribution); ATT.localKeyDistribution = 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; @@ -152,7 +161,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t ATT.setPeerEncryption(connectionHandle, ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::PAIRING_REQUEST); #ifdef _BLE_TRACE_ Serial.print("Peer encryption : 0b"); - Serial.print(ATT.getPeerEncryption(connectionHandle), BIN); + Serial.println(ATT.getPeerEncryption(connectionHandle), BIN); #endif struct __attribute__ ((packed)) PairingResponse { uint8_t code; 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/keyDistribution.cpp b/src/utility/keyDistribution.cpp index 1a9fe099..f754366c 100644 --- a/src/utility/keyDistribution.cpp +++ b/src/utility/keyDistribution.cpp @@ -1,6 +1,6 @@ #include "keyDistribution.h" -KeyDistribution::KeyDistribution(){} +KeyDistribution::KeyDistribution():_octet(0){} KeyDistribution::KeyDistribution(uint8_t octet):_octet(octet){} #define ENCKEY 0b00000001 From 6ef59a51a0bf12a12a76e4311ce6080a9f1bbfc4 Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Sun, 3 Jan 2021 20:02:31 +0000 Subject: [PATCH 05/13] Android bugfix --- .../EncryptedBatteryMonitor.ino | 50 ++++++++++--------- src/utility/ATT.cpp | 5 +- src/utility/HCI.cpp | 7 +-- src/utility/L2CAPSignaling.cpp | 8 +-- 4 files changed, 39 insertions(+), 31 deletions(-) diff --git a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino index 338a374e..d4953b03 100644 --- a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino +++ b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino @@ -44,34 +44,34 @@ void setup() { // IRKs are keys that identify the true owner of a random mac address. // Add IRKs of devices you are bonded with. - BLE.setGetIRKs([](uint8_t* nIRKs, uint8_t** BADDR_TYPES, uint8_t*** BDAddrs, uint8_t*** IRKs){ + BLE.setGetIRKs([](uint8_t* nIRKs, uint8_t** BDaddrTypes, uint8_t*** BDAddrs, uint8_t*** IRKs){ // Set to number of devices *nIRKs = 2; *BDAddrs = new uint8_t*[*nIRKs]; *IRKs = new uint8_t*[*nIRKs]; - *BADDR_TYPES = new uint8_t[*nIRKs]; + *BDaddrTypes = new uint8_t[*nIRKs]; // Set these to the mac and IRK for your bonded devices as printed in the serial console after bonding. - uint8_t iPhoneMac [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPhoneIRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device1Mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device1IRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPadMac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPadIRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, }; + uint8_t device2Mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device2IRK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - (*BADDR_TYPES)[0] = 0; - (*IRKs)[0] = new uint8_t[16]; - memcpy((*IRKs)[0],iPhoneIRK,16); + (*BDaddrTypes)[0] = 0; // Type 0 is for pubc address, type 1 is for static random (*BDAddrs)[0] = new uint8_t[6]; - memcpy((*BDAddrs)[0], iPhoneMac, 6); + (*IRKs)[0] = new uint8_t[16]; + memcpy((*IRKs)[0] , device1IRK,16); + memcpy((*BDAddrs)[0], device1Mac, 6); - (*BADDR_TYPES)[1] = 0; - (*IRKs)[1] = new uint8_t[16]; - memcpy((*IRKs)[1],iPadIRK,16); + (*BDaddrTypes)[1] = 0; (*BDAddrs)[1] = new uint8_t[6]; - memcpy((*BDAddrs)[1], iPadMac, 6); + (*IRKs)[1] = new uint8_t[16]; + memcpy((*IRKs)[1] , device2IRK,16); + memcpy((*BDAddrs)[1], device2Mac, 6); return 1; @@ -83,17 +83,18 @@ void setup() { btct.printBytes(address,6); // Set these to the MAC and LTK of your devices after bonding. - uint8_t iPhoneMac [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPhoneLTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPadMac [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - uint8_t iPadLTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device1Mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device1LTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device2Mac[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + uint8_t device2LTK[16] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - if(memcmp(iPhoneMac, address, 6)==0){ - memcpy(LTK, iPhoneLTK, 16); + if(memcmp(device1Mac, address, 6) == 0) { + memcpy(LTK, device1LTK, 16); + return 1; + }else if(memcmp(device2Mac, address, 6) == 0) { + memcpy(LTK, device2LTK, 16); return 1; - }else if(memcmp(iPadMac, address, 6)==0){ - memcpy(LTK, iPadLTK, 16); } return 0; }); @@ -112,7 +113,8 @@ void setup() { return 1; }); - while(1){// begin initialization + while(1){ + // begin initialization if (!BLE.begin()) { Serial.println("starting BLE failed!"); delay(200); @@ -135,7 +137,7 @@ void setup() { batteryService.addCharacteristic(stringcharacteristic); batteryService.addCharacteristic(secretValue); - BLE.addService(batteryService); // Add the battery service + BLE.addService(batteryService); // Add the battery service batteryLevelChar.writeValue(oldBatteryLevel); // set initial value for this characteristic char* stringCharValue = new char[32]; stringCharValue = "string"; diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index 6ef6bc31..18556413 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -260,7 +260,10 @@ void ATTClass::addConnection(uint16_t handle, uint8_t role, uint8_t peerBdaddrTy Serial.println("Found match."); #endif }else{ - memset(_peers[peerIndex].resolvedAddress, 0, 6); +#ifdef _BLE_TRACE_ + Serial.println("No matching MAC"); +#endif + memset(&_peers[peerIndex].resolvedAddress, 0, 6); } if (_eventHandlers[BLEConnected]) { diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 376cfb9c..4dc78460 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -1218,10 +1218,11 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) uint16_t minLength; uint16_t maxLength; } remoteConnParamReqReply; - memcpy(&remoteConnParamReqReply, &remoteConnParamReq->connectionHandle, sizeof(remoteConnParamReq-1)); + memcpy(&remoteConnParamReqReply, &remoteConnParamReq->connectionHandle, sizeof(RemoteConnParamReq)-1); + remoteConnParamReqReply.minLength = 0x000F; remoteConnParamReqReply.maxLength = 0x0FFF; - sendCommand(OGF_LE_CTL << 10 | 0x20, sizeof(remoteConnParamReqReply), &remoteConnParamReqReply); + sendCommand(OGF_LE_CTL << 10 | 0x20, sizeof(RemoteConnParamReqReply), &remoteConnParamReqReply); break; } case READ_LOCAL_P256_COMPLETE:{ @@ -1381,7 +1382,7 @@ int HCIClass::leEncrypt(uint8_t* key, uint8_t* plaintext, uint8_t* status, uint8 leEncryptCommand.plaintext[15-i] = plaintext[i]; } - int res = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::ENCRYPT, sizeof(leEncryptCommand), &leEncryptCommand); + int res = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::ENCRYPT, 32, &leEncryptCommand); if(res == 0){ #ifdef _BLE_TRACE_ Serial.print("Copying from command Response length: "); diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index 7d9c2c47..f7f93b84 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -144,6 +144,8 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t ATT.localKeyDistribution = KeyDistribution(pairingRequest->responderKeyDistribution); KeyDistribution rkd(pairingRequest->responderKeyDistribution); AuthReq req(pairingRequest->authReq); + KeyDistribution responseKD = KeyDistribution(); + responseKD.setIdKey(true); #ifdef _BLE_TRACE_ Serial.print("Req has properties: "); Serial.print(req.Bonding()?"bonding, ":"no bonding, "); @@ -171,7 +173,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t maxEncSize; uint8_t initiatorKeyDistribution; uint8_t responderKeyDistribution; - } response = { CONNECTION_PAIRING_RESPONSE, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 0x10, 0b1011, 0b1011}; + } response = { CONNECTION_PAIRING_RESPONSE, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 0x10, responseKD.getOctet(), responseKD.getOctet()}; HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); } @@ -316,9 +318,9 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t Serial.println("Calculate f5, f6:"); Serial.print("DH : "); btct.printBytes(HCI.DHKey,32); - Serial.println("Na : "); + Serial.print("Na : "); btct.printBytes(HCI.Na,16); - Serial.println("Nb : "); + Serial.print("Nb : "); btct.printBytes(HCI.Nb,16); Serial.print("MAC : "); btct.printBytes(MacKey,16); From 0aa2e87a7c322931793657b8096afd8930a312d4 Mon Sep 17 00:00:00 2001 From: eltos Date: Sun, 3 Jan 2021 23:25:59 +0100 Subject: [PATCH 06/13] Handle remote DHKey confirmation before own DHKey --- src/utility/HCI.cpp | 74 +++++++++++++++++++++++++++++++++- src/utility/HCI.h | 1 + src/utility/L2CAPSignaling.cpp | 12 +++++- 3 files changed, 83 insertions(+), 4 deletions(-) diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 4dc78460..014e25a3 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -1345,13 +1345,83 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) #endif encryption |= PEER_ENCRYPTION::DH_KEY_CALULATED; ATT.setPeerEncryption(connectionHandle, encryption); + + if((encryption & PEER_ENCRYPTION::RECEIVED_DH_CHECK) > 0){ +#ifdef _BLE_TRACE_ + Serial.println("Recieved DHKey check already so calculate f5, f6 now."); +#endif + + uint8_t BD_ADDR_REMOTE[7]; + ATT.getPeerAddrWithType(connectionHandle, BD_ADDR_REMOTE); + + + uint8_t MacKey[16]; + uint8_t localAddress[7]; + + memcpy(&localAddress[1],HCI.localAddr,6); + localAddress[0] = 0; // IOT 33 uses a static address + + btct.f5(HCI.DHKey,HCI.Na,HCI.Nb,BD_ADDR_REMOTE,localAddress,MacKey,HCI.LTK); + + uint8_t Ea[16]; + uint8_t Eb[16]; + uint8_t R[16]; + uint8_t MasterIOCap[3]; + uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP}; + + ATT.getPeerIOCap(connectionHandle, MasterIOCap); + for(int i=0; i<16; i++) R[i] = 0; + + btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, BD_ADDR_REMOTE, localAddress, Ea); + btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, BD_ADDR_REMOTE, Eb); + + #ifdef _BLE_TRACE_ - if(encryption | PEER_ENCRYPTION::RECEIVED_DH_CHECK){ - Serial.println("Recieved DHKey check already so calculate f5, f6."); + Serial.println("Calculate f5, f6:"); + Serial.print("DH : "); + btct.printBytes(HCI.DHKey,32); + Serial.print("Na : "); + btct.printBytes(HCI.Na,16); + Serial.print("Nb : "); + btct.printBytes(HCI.Nb,16); + Serial.print("MAC : "); + btct.printBytes(MacKey,16); + // Serial.print("Expected MAC: "); + // printBytes(EXPECTED_MAC, 16); + Serial.print("LTK : "); + btct.printBytes(HCI.LTK,16); + // Serial.print("Expected LTK: "); + // printBytes(EXPECTED_LTK, 16); + Serial.print("Expected Ex : "); + btct.printBytes(HCI.remoteDHKeyCheckBuffer, 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(BD_ADDR_REMOTE, 7); + Serial.print("MasterIOCAP : "); + btct.printBytes(MasterIOCap, 3); + Serial.println("Send Eb Back."); +#endif + uint8_t ret[17]; + ret[0] = 0x0d; + for(int i=0; istatus, HEX); #endif diff --git a/src/utility/HCI.h b/src/utility/HCI.h index de15bdf8..4ed9e82e 100644 --- a/src/utility/HCI.h +++ b/src/utility/HCI.h @@ -111,6 +111,7 @@ class HCIClass { // TODO: Send command be private again & use ATT implementation within ATT. virtual int sendCommand(uint16_t opcode, uint8_t plen = 0, void* parameters = NULL); uint8_t remotePublicKeyBuffer[64]; + uint8_t remoteDHKeyCheckBuffer[16]; uint8_t Na[16]; uint8_t Nb[16]; uint8_t DHKey[32]; diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index f7f93b84..60152a7d 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -290,7 +290,14 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t HCI.readBdAddr(); ATT.setPeerEncryption(connectionHandle, encryptionState); - if((encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED) > 0){ + if((encryptionState & PEER_ENCRYPTION::DH_KEY_CALULATED) == 0){ +#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. uint8_t MacKey[16]; @@ -312,7 +319,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, BD_ADDR_REMOTE, localAddress, Ea); btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, BD_ADDR_REMOTE, Eb); - + #ifdef _BLE_TRACE_ Serial.println("Calculate f5, f6:"); @@ -353,6 +360,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t ret[sizeof(Eb)-i] = Eb[i]; } HCI.sendAclPkt(connectionHandle, 0x06, sizeof(ret), ret ); + ATT.setPeerEncryption(connectionHandle, encryptionState | PEER_ENCRYPTION::SENT_DH_CHECK); } } } From 750b7f179546520f6929db2485fd40ba747ebe2f Mon Sep 17 00:00:00 2001 From: eltos Date: Sun, 3 Jan 2021 23:45:38 +0100 Subject: [PATCH 07/13] Actually check remote DHKey --- src/utility/HCI.cpp | 35 +++++++++++++++++++++++++++------ src/utility/L2CAPSignaling.cpp | 36 +++++++++++++++++++++++++++------- 2 files changed, 58 insertions(+), 13 deletions(-) diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 014e25a3..157ed7d3 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -24,6 +24,8 @@ #include "btct.h" #include "HCI.h" +//#define _BLE_TRACE_ + #define HCI_COMMAND_PKT 0x01 #define HCI_ACLDATA_PKT 0x02 #define HCI_EVENT_PKT 0x04 @@ -1408,13 +1410,34 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) btct.printBytes(MasterIOCap, 3); Serial.println("Send Eb Back."); #endif - uint8_t ret[17]; - ret[0] = 0x0d; - for(int i=0; i Date: Mon, 4 Jan 2021 11:07:36 +0100 Subject: [PATCH 08/13] Move LTK signaling and confirm to separate method --- src/utility/HCI.cpp | 89 +------------------ src/utility/HCI.h | 2 + src/utility/L2CAPSignaling.cpp | 150 +++++++++++++++------------------ src/utility/L2CAPSignaling.h | 6 ++ 4 files changed, 80 insertions(+), 167 deletions(-) diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 157ed7d3..4c97f79d 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -1337,7 +1337,6 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) break; } - uint8_t encryption = ATT.getPeerEncryption(connectionHandle); for(int i=0; i<32; i++) DHKey[31-i] = evtLeDHKeyComplete->DHKey[i]; @@ -1345,98 +1344,14 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) Serial.println("Stored our DHKey:"); btct.printBytes(DHKey,32); #endif - encryption |= PEER_ENCRYPTION::DH_KEY_CALULATED; + 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("Recieved DHKey check already so calculate f5, f6 now."); #endif - - uint8_t BD_ADDR_REMOTE[7]; - ATT.getPeerAddrWithType(connectionHandle, BD_ADDR_REMOTE); - - - uint8_t MacKey[16]; - uint8_t localAddress[7]; - - memcpy(&localAddress[1],HCI.localAddr,6); - localAddress[0] = 0; // IOT 33 uses a static address - - btct.f5(HCI.DHKey,HCI.Na,HCI.Nb,BD_ADDR_REMOTE,localAddress,MacKey,HCI.LTK); - - uint8_t Ea[16]; - uint8_t Eb[16]; - uint8_t R[16]; - uint8_t MasterIOCap[3]; - uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP}; - - ATT.getPeerIOCap(connectionHandle, MasterIOCap); - for(int i=0; i<16; i++) R[i] = 0; - - btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, BD_ADDR_REMOTE, localAddress, Ea); - btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, BD_ADDR_REMOTE, Eb); - - -#ifdef _BLE_TRACE_ - Serial.println("Calculate f5, f6:"); - Serial.print("DH : "); - btct.printBytes(HCI.DHKey,32); - Serial.print("Na : "); - btct.printBytes(HCI.Na,16); - Serial.print("Nb : "); - btct.printBytes(HCI.Nb,16); - Serial.print("MAC : "); - btct.printBytes(MacKey,16); - // Serial.print("Expected MAC: "); - // printBytes(EXPECTED_MAC, 16); - Serial.print("LTK : "); - btct.printBytes(HCI.LTK,16); - // Serial.print("Expected LTK: "); - // printBytes(EXPECTED_LTK, 16); - Serial.print("Expected Ex : "); - btct.printBytes(HCI.remoteDHKeyCheckBuffer, 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(BD_ADDR_REMOTE, 7); - Serial.print("MasterIOCAP : "); - btct.printBytes(MasterIOCap, 3); - Serial.println("Send Eb Back."); -#endif - // Check if RemoteDHKeyCheck = Ea - bool EaCheck = true; - for(int i = 0; i < 16; i++){ - if (Ea[i] != HCI.remoteDHKeyCheckBuffer[i]){ - EaCheck = false; - } - } - - if (EaCheck){ - // Send our confirmation value to complete authentication stage 2 - uint8_t ret[17]; - ret[0] = CONNECTION_PAIRING_DHKEY_CHECK; - for(int i=0; i +#include "L2CAPSignaling.h" + #define OGF_LINK_CTL 0x01 #define OGF_HOST_CTL 0x03 #define OGF_INFO_PARAM 0x04 diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index 322bd13d..4adeaa0d 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -278,11 +278,8 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t else if(code == CONNECTION_PAIRING_DHKEY_CHECK) { uint8_t RemoteDHKeyCheck[16]; - uint8_t BD_ADDR_REMOTE[7]; - ATT.getPeerAddrWithType(connectionHandle, BD_ADDR_REMOTE); for(int i=0; i<16; i++) RemoteDHKeyCheck[15-i] = l2capSignalingHdr->data[i]; - uint8_t encryptionState = ATT.getPeerEncryption(connectionHandle) | PEER_ENCRYPTION::RECEIVED_DH_CHECK; - + #ifdef _BLE_TRACE_ Serial.println("[Info] DH Key check"); @@ -290,7 +287,9 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t btct.printBytes(RemoteDHKeyCheck, 16); #endif - HCI.readBdAddr(); + + + 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_ @@ -301,89 +300,80 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t } else { // We've already calculated the DHKey so we can calculate our check and send it. + smCalculateLTKandConfirm(connectionHandle, RemoteDHKeyCheck); - uint8_t MacKey[16]; - uint8_t localAddress[7]; - - memcpy(&localAddress[1],HCI.localAddr,6); - localAddress[0] = 0; // IOT 33 uses a static address + } + } +} - btct.f5(HCI.DHKey,HCI.Na,HCI.Nb,BD_ADDR_REMOTE,localAddress,MacKey,HCI.LTK); - - uint8_t Ea[16]; - uint8_t Eb[16]; - uint8_t R[16]; - uint8_t MasterIOCap[3]; - uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP}; - - ATT.getPeerIOCap(connectionHandle, MasterIOCap); - for(int i=0; i<16; i++) R[i] = 0; - - btct.f6(MacKey, HCI.Na,HCI.Nb,R, MasterIOCap, BD_ADDR_REMOTE, localAddress, Ea); - btct.f6(MacKey, HCI.Nb,HCI.Na,R, SlaveIOCap, localAddress, BD_ADDR_REMOTE, Eb); - +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] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP}; + + 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 f5, f6:"); - Serial.print("DH : "); - btct.printBytes(HCI.DHKey,32); - Serial.print("Na : "); - btct.printBytes(HCI.Na,16); - Serial.print("Nb : "); - btct.printBytes(HCI.Nb,16); - Serial.print("MAC : "); - btct.printBytes(MacKey,16); - // Serial.print("Expected MAC: "); - // printBytes(EXPECTED_MAC, 16); - Serial.print("LTK : "); - btct.printBytes(HCI.LTK,16); - // Serial.print("Expected LTK: "); - // printBytes(EXPECTED_LTK, 16); - Serial.print("Expected Ex : "); - btct.printBytes(RemoteDHKeyCheck, 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(BD_ADDR_REMOTE, 7); - Serial.print("MasterIOCAP : "); - btct.printBytes(MasterIOCap, 3); - Serial.println("Send Eb Back."); + 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 RemoteDHKeyCheck = Ea - bool EaCheck = true; - for(int i = 0; i < 16; i++){ - if (Ea[i] != RemoteDHKeyCheck[i]){ - EaCheck = false; - } - } - - if (EaCheck){ - // Send our confirmation value to complete authentication stage 2 - uint8_t ret[17]; - ret[0] = CONNECTION_PAIRING_DHKEY_CHECK; - for(int i=0; i Date: Mon, 4 Jan 2021 12:20:06 +0100 Subject: [PATCH 09/13] Add method to control and query pairing --- .../EncryptedBatteryMonitor.ino | 67 +++++++---- keywords.txt | 3 + src/local/BLELocalDevice.cpp | 26 +++++ src/local/BLELocalDevice.h | 10 ++ src/utility/ATT.cpp | 26 +++++ src/utility/ATT.h | 2 + src/utility/L2CAPSignaling.cpp | 105 +++++++++++------- src/utility/L2CAPSignaling.h | 13 ++- 8 files changed, 190 insertions(+), 62 deletions(-) diff --git a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino index d4953b03..fd40f5ee 100644 --- a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino +++ b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino @@ -18,6 +18,14 @@ #include +#define PAIR_BUTTON D3 // 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"); @@ -31,13 +39,17 @@ BLEStringCharacteristic stringcharacteristic("183E", BLERead | BLEWrite, 31); BLEUnsignedCharCharacteristic secretValue("2a3F", BLERead | BLEWrite | BLEEncryption); int oldBatteryLevel = 0; // last battery level reading from analog input -long previousMillis = 0; // last time the battery level was checked, in ms +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; void setup() { Serial.begin(9600); // initialize serial communication while (!Serial); - pinMode(LED_BUILTIN, OUTPUT); // initialize the built-in LED pin to indicate when a central is connected + 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"); @@ -145,6 +157,9 @@ void setup() { 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 @@ -169,30 +184,44 @@ 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) { - Serial.print("Connected to central: "); - // print the central's BT address: - Serial.println(central.address()); + 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: - while (central.connected()) { - long currentMillis = millis(); - // if 200ms have passed, check the battery level: - if (currentMillis - previousMillis >= 1000) { - previousMillis = currentMillis; - updateBatteryLevel(); - if(secretValue.value()>0){ - digitalWrite(13,HIGH); - }else{ - digitalWrite(13,LOW); - } - } + 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() { @@ -208,4 +237,4 @@ void updateBatteryLevel() { 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/local/BLELocalDevice.cpp b/src/local/BLELocalDevice.cpp index 49cc6603..029fb488 100644 --- a/src/local/BLELocalDevice.cpp +++ b/src/local/BLELocalDevice.cpp @@ -214,6 +214,16 @@ bool BLELocalDevice::connected() const return ATT.connected(); } +/* + * Whether there is at least one paired device + */ +bool BLELocalDevice::paired() +{ + HCI.poll(); + + return ATT.paired(); +} + bool BLELocalDevice::disconnect() { return ATT.disconnect(); @@ -395,6 +405,22 @@ void BLELocalDevice::setTimeout(unsigned long timeout) ATT.setTimeout(timeout); } +/* + * Control whether pairing is allowed or rejected + * Use true/false or the Pairable enum + */ +void BLELocalDevice::setPairable(uint8_t pairable) +{ + L2CAPSignaling.setPairingEnabled(pairable); +} + +/* + * Whether pairing is currently allowed + */ +bool BLELocalDevice::pairable() +{ + return L2CAPSignaling.isPairingEnabled(); +} void BLELocalDevice::setGetIRKs(int (*getIRKs)(uint8_t* nIRKs, uint8_t** BADDR_type, uint8_t*** BADDRs, uint8_t*** IRKs)){ HCI._getIRKs = getIRKs; diff --git a/src/local/BLELocalDevice.h b/src/local/BLELocalDevice.h index 22a5f589..0c471938 100644 --- a/src/local/BLELocalDevice.h +++ b/src/local/BLELocalDevice.h @@ -24,6 +24,12 @@ #include "BLEService.h" #include "BLEAdvertisingData.h" +enum Pairable { + NO = 0, + YES = 1, + ONCE = 2, +}; + class BLELocalDevice { public: BLELocalDevice(); @@ -80,6 +86,10 @@ class BLELocalDevice { virtual void debug(Stream& stream); virtual void noDebug(); + + virtual void setPairable(uint8_t pairable); + virtual bool pairable(); + virtual bool paired(); /// TODO: Put in actual variable names virtual void setStoreIRK(int (*storeIRK)(uint8_t*, uint8_t*)); diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index 18556413..a5166217 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -497,6 +497,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++) { diff --git a/src/utility/ATT.h b/src/utility/ATT.h index 1c8910c3..c9f007f9 100644 --- a/src/utility/ATT.h +++ b/src/utility/ATT.h @@ -75,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(); diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index 4adeaa0d..622f2178 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -32,6 +32,7 @@ L2CAPSignalingClass::L2CAPSignalingClass() : _minInterval(0), _maxInterval(0), _supervisionTimeout(0) + ,_pairing_enabled(1) { } @@ -131,53 +132,64 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t btct.printBytes(data,dlen); #endif if (code == CONNECTION_PAIRING_REQUEST) { - // 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; - - - ATT.remoteKeyDistribution = KeyDistribution(pairingRequest->initiatorKeyDistribution); - ATT.localKeyDistribution = KeyDistribution(pairingRequest->responderKeyDistribution); - KeyDistribution rkd(pairingRequest->responderKeyDistribution); - AuthReq req(pairingRequest->authReq); - KeyDistribution responseKD = KeyDistribution(); - responseKD.setIdKey(true); + + 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; + + + ATT.remoteKeyDistribution = KeyDistribution(pairingRequest->initiatorKeyDistribution); + ATT.localKeyDistribution = KeyDistribution(pairingRequest->responderKeyDistribution); + KeyDistribution rkd(pairingRequest->responderKeyDistribution); + AuthReq req(pairingRequest->authReq); + KeyDistribution responseKD = KeyDistribution(); + responseKD.setIdKey(true); #ifdef _BLE_TRACE_ - Serial.print("Req has properties: "); - Serial.print(req.Bonding()?"bonding, ":"no bonding, "); - 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, "); + 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); + 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); + 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, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 0x10, responseKD.getOctet(), responseKD.getOctet()}; - - HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); + 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, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 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) { @@ -392,6 +404,15 @@ void L2CAPSignalingClass::setSupervisionTimeout(uint16_t supervisionTimeout) _supervisionTimeout = supervisionTimeout; } +void L2CAPSignalingClass::setPairingEnabled(uint8_t enabled) +{ + _pairing_enabled = enabled; +} +bool L2CAPSignalingClass::isPairingEnabled() +{ + return _pairing_enabled > 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 7fc64d4f..f8a361ee 100644 --- a/src/utility/L2CAPSignaling.h +++ b/src/utility/L2CAPSignaling.h @@ -41,8 +41,15 @@ #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 0x3 +#define LOCAL_IOCAP IOCAP_NO_INPUT_NO_OUTPUT // will use JustWorks pairing class L2CAPSignalingClass { public: @@ -63,6 +70,9 @@ class L2CAPSignalingClass { virtual void setConnectionInterval(uint16_t minInterval, uint16_t maxInterval); virtual void setSupervisionTimeout(uint16_t supervisionTimeout); + + virtual void setPairingEnabled(uint8_t enabled); + virtual bool isPairingEnabled(); @@ -78,6 +88,7 @@ class L2CAPSignalingClass { uint16_t _minInterval; uint16_t _maxInterval; uint16_t _supervisionTimeout; + uint8_t _pairing_enabled; }; extern L2CAPSignalingClass& L2CAPSignaling; From d9fb4bcfbf394d98aac93b9b3d4b087552cf4a22 Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Mon, 4 Jan 2021 19:04:01 +0000 Subject: [PATCH 10/13] Secure random & reject unknown LTK --- src/local/BLELocalDevice.h | 18 +++-- src/utility/HCI.cpp | 132 +++++++++++++++++++++++---------- src/utility/HCI.h | 30 +++++--- src/utility/L2CAPSignaling.cpp | 18 +++-- 4 files changed, 137 insertions(+), 61 deletions(-) diff --git a/src/local/BLELocalDevice.h b/src/local/BLELocalDevice.h index 22a5f589..d06664da 100644 --- a/src/local/BLELocalDevice.h +++ b/src/local/BLELocalDevice.h @@ -81,11 +81,19 @@ class BLELocalDevice { virtual void debug(Stream& stream); virtual void noDebug(); -/// TODO: Put in actual variable names - virtual void setStoreIRK(int (*storeIRK)(uint8_t*, uint8_t*)); - virtual void setGetIRKs(int (*getIRKs)(uint8_t* nIRKs, uint8_t** BADDR_type, uint8_t*** BADDRs, uint8_t*** IRKs)); - virtual void setStoreLTK(int (*storeLTK)(uint8_t*, uint8_t*)); - virtual void setGetLTK(int (*getLTK)(uint8_t*, uint8_t*)); + // address - The mac to store + // IRK - The IRK to store with this mac + virtual void setStoreIRK(int (*storeIRK)(uint8_t* address, uint8_t* IRK)); + // nIRKs - the number of IRKs being provided. + // BDAddrType - an array containing the type of each address (0 public, 1 static random) + // BDAddrs - an array containing the list of addresses + virtual void setGetIRKs(int (*getIRKs)(uint8_t* nIRKs, uint8_t** BDAddrType, uint8_t*** BDAddrs, uint8_t*** IRKs)); + // address - the address to store [6 bytes] + // LTK - the LTK to store with this mac [16 bytes] + virtual void setStoreLTK(int (*storeLTK)(uint8_t* address, uint8_t* LTK)); + // address - The mac address needing its LTK + // LTK - 16 octet LTK for the mac address + virtual void setGetLTK(int (*getLTK)(uint8_t* address, uint8_t* LTK)); uint8_t BDaddress[6]; protected: diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 4dc78460..1a604d79 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -23,6 +23,8 @@ #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 @@ -1139,50 +1141,63 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) // Load our LTK for this connection. uint8_t peerAddr[7]; uint8_t resolvableAddr[6]; + uint8_t foundLTK; ATT.getPeerAddrWithType(ltkRequest->connectionHandle, peerAddr); - if(ATT.getPeerResolvedAddress(ltkRequest->connectionHandle, resolvableAddr) - && !((ATT.getPeerEncryption(ltkRequest->connectionHandle) & PEER_ENCRYPTION::PAIRING_REQUEST)>0)){ - _getLTK(resolvableAddr, HCI.LTK); + if((ATT.getPeerEncryption(ltkRequest->connectionHandle) & PEER_ENCRYPTION::PAIRING_REQUEST)>0){ + // Pairing request - LTK is one in buffer already + foundLTK = 1; }else{ - _getLTK(&peerAddr[1], HCI.LTK); + if(ATT.getPeerResolvedAddress(ltkRequest->connectionHandle, resolvableAddr)){ + foundLTK = getLTK(resolvableAddr, HCI.LTK); + }else{ + foundLTK = getLTK(&peerAddr[1], HCI.LTK); + } } - // } + // } //2d // Send our LTK back - 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 + if(foundLTK){ + struct __attribute__ ((packed)) LTKReply { - 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 + 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.print("Failed to send LTK...: "); - btct.printBytes((uint8_t*)&result,2); + Serial.println("LTK not found, rejecting"); #endif + sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::LONG_TERM_KEY_NEGATIVE_REPLY,2, <kRequest->connectionHandle); } break; } @@ -1256,10 +1271,10 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) uint8_t Z = 0; - for(int i=0; i<16; i++){ - /// TODO: Implement secure random - Nb[i] = rand(); //// Should use ESP or ECCx08 - } + + HCI.leRand(Nb); + HCI.leRand(&Nb[8]); + #ifdef _BLE_TRACE_ Serial.print("nb: "); btct.printBytes(Nb, 16); @@ -1405,6 +1420,47 @@ int HCIClass::leEncrypt(uint8_t* key, uint8_t* plaintext, uint8_t* status, uint8 #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; + } +} + +/// 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 de15bdf8..b8671303 100644 --- a/src/utility/HCI.h +++ b/src/utility/HCI.h @@ -21,6 +21,7 @@ #define _HCI_H_ #include +#include "bitDescriptions.h" #define OGF_LINK_CTL 0x01 #define OGF_HOST_CTL 0x03 @@ -29,11 +30,13 @@ #define OGF_LE_CTL 0x08 enum LE_COMMAND { - ENCRYPT = 0x0017, - LONG_TERM_KEY_REPLY = 0x001A, - READ_LOCAL_P256 = 0x0025, - GENERATE_DH_KEY_V1 = 0x0026, - GENERATE_DH_KEY_V2 = 0x005E + 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, @@ -89,6 +92,9 @@ class HCIClass { 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 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); @@ -108,7 +114,7 @@ class HCIClass { virtual void debug(Stream& stream); virtual void noDebug(); - // TODO: Send command be private again & use ATT implementation within ATT. + // 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 Na[16]; @@ -116,11 +122,13 @@ class HCIClass { uint8_t DHKey[32]; uint8_t localAddr[6]; uint8_t LTK[16]; - - int (*_storeIRK)(uint8_t* address, uint8_t* peerIrk); - int (*_getIRKs)(uint8_t* nIRKs,uint8_t** BADDR_type, uint8_t*** BADDRs, uint8_t*** IRKs); - int (*_storeLTK)(uint8_t*, uint8_t*); - int (*_getLTK)(uint8_t*, uint8_t*); + 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; private: diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index f7f93b84..9d9efd81 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -26,6 +26,9 @@ #define CONNECTION_PARAMETER_UPDATE_REQUEST 0x12 #define CONNECTION_PARAMETER_UPDATE_RESPONSE 0x13 + +// #define _BLE_TRACE_ + L2CAPSignalingClass::L2CAPSignalingClass() : _minInterval(0), _maxInterval(0), @@ -139,13 +142,14 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t responderKeyDistribution; } *pairingRequest = (PairingRequest*)l2capSignalingHdr->data; - - ATT.remoteKeyDistribution = KeyDistribution(pairingRequest->initiatorKeyDistribution); - ATT.localKeyDistribution = KeyDistribution(pairingRequest->responderKeyDistribution); - KeyDistribution rkd(pairingRequest->responderKeyDistribution); - AuthReq req(pairingRequest->authReq); 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, "); @@ -173,7 +177,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t maxEncSize; uint8_t initiatorKeyDistribution; uint8_t responderKeyDistribution; - } response = { CONNECTION_PAIRING_RESPONSE, LOCAL_IOCAP, 0, LOCAL_AUTHREQ, 0x10, responseKD.getOctet(), responseKD.getOctet()}; + } response = { CONNECTION_PAIRING_RESPONSE, LOCAL_IOCAP, 0, HCI.localAuthreq().getOctet(), 0x10, responseKD.getOctet(), responseKD.getOctet()}; HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); } @@ -305,7 +309,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t Eb[16]; uint8_t R[16]; uint8_t MasterIOCap[3]; - uint8_t SlaveIOCap[3] = {LOCAL_AUTHREQ, 0x0, LOCAL_IOCAP}; + uint8_t SlaveIOCap[3] = {HCI.localAuthreq().getOctet(), 0x0, LOCAL_IOCAP}; ATT.getPeerIOCap(connectionHandle, MasterIOCap); for(int i=0; i<16; i++) R[i] = 0; From e2868a7108fec2bd6349c48ee556e5fe446daad0 Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Thu, 7 Jan 2021 10:43:23 +0000 Subject: [PATCH 11/13] Pairing code & binary confirm callbacks --- src/local/BLELocalDevice.cpp | 6 ++++ src/local/BLELocalDevice.h | 3 ++ src/utility/ATT.cpp | 2 ++ src/utility/HCI.cpp | 15 +++++++++- src/utility/HCI.h | 4 +++ src/utility/L2CAPSignaling.cpp | 54 ++++++++++++++++++++++++++++++++-- src/utility/L2CAPSignaling.h | 2 +- src/utility/btct.cpp | 36 +++++++++++++++++++++++ src/utility/btct.h | 2 ++ 9 files changed, 120 insertions(+), 4 deletions(-) diff --git a/src/local/BLELocalDevice.cpp b/src/local/BLELocalDevice.cpp index 029fb488..d83c1cb3 100644 --- a/src/local/BLELocalDevice.cpp +++ b/src/local/BLELocalDevice.cpp @@ -434,6 +434,12 @@ void BLELocalDevice::setStoreLTK(int (*storeLTK)(uint8_t*, uint8_t*)){ void BLELocalDevice::setStoreIRK(int (*storeIRK)(uint8_t*, uint8_t*)){ HCI._storeIRK = storeIRK; } +void BLELocalDevice::setDisplayCode(void (*displayCode)(uint32_t confirmationCode)){ + HCI._displayCode = displayCode; +} +void BLELocalDevice::setBinaryConfirmPairing(bool (*binaryConfirmPairing)()){ + HCI._binaryConfirmPairing = binaryConfirmPairing; +} void BLELocalDevice::debug(Stream& stream) { diff --git a/src/local/BLELocalDevice.h b/src/local/BLELocalDevice.h index 75e7dc31..6c45c063 100644 --- a/src/local/BLELocalDevice.h +++ b/src/local/BLELocalDevice.h @@ -104,6 +104,9 @@ class BLELocalDevice { // address - The mac address needing its LTK // LTK - 16 octet LTK for the mac address virtual void setGetLTK(int (*getLTK)(uint8_t* address, uint8_t* LTK)); + + virtual void setDisplayCode(void (*displayCode)(uint32_t confirmationCode)); + virtual void setBinaryConfirmPairing(bool (*binaryConfirmPairing)()); uint8_t BDaddress[6]; protected: diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index a5166217..91ffeed0 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -81,6 +81,8 @@ #define ATT_ECODE_UNSUPP_GRP_TYPE 0x10 #define ATT_ECODE_INSUFF_RESOURCES 0x11 +// #define _BLE_TRACE_ + ATTClass::ATTClass() : _maxMtu(23), _timeout(5000), diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 9bcec4e1..82303ed3 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -26,7 +26,6 @@ #include "bitDescriptions.h" // #define _BLE_TRACE_ -//#define _BLE_TRACE_ #define HCI_COMMAND_PKT 0x01 #define HCI_ACLDATA_PKT 0x02 @@ -1258,6 +1257,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) uint8_t publicKey[64]; } pairingPublicKey = {CONNECTION_PAIRING_PUBLIC_KEY,0}; memcpy(pairingPublicKey.publicKey,evtReadLocalP256Complete->localPublicKey,64); + memcpy(localPublicKeyBuffer, evtReadLocalP256Complete->localPublicKey,64); // Send the local public key to the remote uint16_t connectionHandle = ATT.getPeerEncrptingConnectionHandle(); @@ -1456,6 +1456,19 @@ int HCIClass::storeLTK(uint8_t* address, uint8_t* LTK){ return 0; } } +uint8_t HCIClass::localIOCap(){ + if(_displayCode!=0){ + /// We have a display + if(_binaryConfirmPairing!=0){ + return IOCAP_DISPLAY_YES_NO; + }else{ + return IOCAP_DISPLAY_ONLY; + } + }else{ + // We have no display + return IOCAP_NO_INPUT_NO_OUTPUT; + } +} /// Stub function to generate parameters for local authreq AuthReq HCIClass::localAuthreq(){ diff --git a/src/utility/HCI.h b/src/utility/HCI.h index f2bc535e..af46265a 100644 --- a/src/utility/HCI.h +++ b/src/utility/HCI.h @@ -97,6 +97,7 @@ class HCIClass { // Generate a 64 bit random number virtual int leRand(uint8_t rand[]); virtual AuthReq localAuthreq(); + virtual uint8_t localIOCap(); virtual int saveNewAddress(uint8_t addressType, uint8_t* address, uint8_t* peerIrk, uint8_t* remoteIrk); virtual int leAddResolvingAddress(uint8_t addressType, uint8_t* address, uint8_t* peerIrk, uint8_t* remoteIrk); @@ -119,6 +120,7 @@ class HCIClass { // TODO: Send command be private again & use ATT implementation of send command within ATT. virtual int sendCommand(uint16_t opcode, uint8_t plen = 0, void* parameters = NULL); uint8_t remotePublicKeyBuffer[64]; + uint8_t localPublicKeyBuffer[64]; uint8_t remoteDHKeyCheckBuffer[16]; uint8_t Na[16]; uint8_t Nb[16]; @@ -132,6 +134,8 @@ class HCIClass { int (*_getIRKs)(uint8_t* nIRKs,uint8_t** BADDR_type, uint8_t*** BADDRs, uint8_t*** IRKs) = 0; int (*_storeLTK)(uint8_t*, uint8_t*) = 0; int (*_getLTK)(uint8_t*, uint8_t*) = 0; + void (*_displayCode)(uint32_t confirmationCode) = 0; + bool (*_binaryConfirmPairing)() = 0; private: diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index f7712051..2ef08b62 100644 --- a/src/utility/L2CAPSignaling.cpp +++ b/src/utility/L2CAPSignaling.cpp @@ -180,7 +180,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t uint8_t maxEncSize; uint8_t initiatorKeyDistribution; uint8_t responderKeyDistribution; - } response = { CONNECTION_PAIRING_RESPONSE, LOCAL_IOCAP, 0, HCI.localAuthreq().getOctet(), 0x10, responseKD.getOctet(), responseKD.getOctet()}; + } response = { CONNECTION_PAIRING_RESPONSE, HCI.localIOCap(), 0, HCI.localAuthreq().getOctet(), 0x10, responseKD.getOctet(), responseKD.getOctet()}; HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); @@ -209,6 +209,55 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t for(int i=0; i< 16; i++) response.Nb[15-i] = HCI.Nb[i]; HCI.sendAclPkt(connectionHandle, SECURITY_CID, sizeof(response), &response); + + // We now have all needed for compare value + uint8_t g2Result[4]; + uint8_t U[32]; + uint8_t V[32]; + + for(int i=0; i<32; i++){ + U[31-i] = HCI.remotePublicKeyBuffer[i]; + V[31-i] = HCI.localPublicKeyBuffer[i]; + } + + btct.g2(U,V,HCI.Na,HCI.Nb, g2Result); + uint32_t result = 0; + for(int i=0; i<4; i++) result += g2Result[3-i] << 8*i; + +#ifdef _BLE_TRACE_ + Serial.print("U : "); + btct.printBytes(U,32); + Serial.print("V : "); + btct.printBytes(V,32); + Serial.print("X : "); + btct.printBytes(X,16); + Serial.print("Y : "); + btct.printBytes(Y,16); + Serial.print("g2res : "); + btct.printBytes(g2Result,4); + Serial.print("Result : "); + Serial.println(result); +#endif + + if(HCI._displayCode!=0){ + HCI._displayCode(result%1000000); + } + if(HCI._binaryConfirmPairing!=0){ + if(!HCI._binaryConfirmPairing()){ +#ifdef _BLE_TRACE_ + Serial.println("User rejection"); +#endif + uint8_t rejection[2]; + rejection[0] = CONNECTION_PAIRING_FAILED; + rejection[1] = 0x0C; // Numeric comparison failed + HCI.sendAclPkt(connectionHandle, SECURITY_CID, 2, rejection); + ATT.setPeerEncryption(connectionHandle, PEER_ENCRYPTION::NO_ENCRYPTION); + }else{ +#ifdef _BLE_TRACE_ + Serial.println("User did confirm"); +#endif + } + } } else if (code == CONNECTION_PAIRING_RESPONSE) { @@ -224,6 +273,7 @@ void L2CAPSignalingClass::handleSecurityData(uint16_t connectionHandle, uint8_t Serial.print("Pairing failed with code: 0x"); Serial.println(pairingFailed->reason,HEX); #endif + ATT.setPeerEncryption(connectionHandle, PEER_ENCRYPTION::NO_ENCRYPTION); } else if (code == CONNECTION_IDENTITY_INFORMATION){ struct __attribute__ ((packed)) IdentityInformation { @@ -338,7 +388,7 @@ void L2CAPSignalingClass::smCalculateLTKandConfirm(uint16_t handle, uint8_t expe uint8_t Eb[16]; uint8_t R[16]; uint8_t MasterIOCap[3]; - uint8_t SlaveIOCap[3] = {HCI.localAuthreq().getOctet(), 0x0, LOCAL_IOCAP}; + uint8_t SlaveIOCap[3] = {HCI.localAuthreq().getOctet(), 0x0, HCI.localIOCap()}; ATT.getPeerIOCap(handle, MasterIOCap); for(int i=0; i<16; i++) R[i] = 0; diff --git a/src/utility/L2CAPSignaling.h b/src/utility/L2CAPSignaling.h index f8a361ee..26042167 100644 --- a/src/utility/L2CAPSignaling.h +++ b/src/utility/L2CAPSignaling.h @@ -49,7 +49,7 @@ #define LOCAL_AUTHREQ 0b00101101 -#define LOCAL_IOCAP IOCAP_NO_INPUT_NO_OUTPUT // will use JustWorks pairing +// #define LOCAL_IOCAP IOCAP_DISPLAY_ONLY // will use JustWorks pairing class L2CAPSignalingClass { public: diff --git a/src/utility/btct.cpp b/src/utility/btct.cpp index d78e328e..b4faf053 100644 --- a/src/utility/btct.cpp +++ b/src/utility/btct.cpp @@ -150,6 +150,42 @@ void BluetoothCryptoToolbox::testAh() printBytes(ourResult, 3); } +int BluetoothCryptoToolbox::g2(uint8_t U[], uint8_t V[], uint8_t X[], uint8_t Y[], uint8_t out[4]) +{ + struct __attribute__ ((packed)) CmacInput { + uint8_t U[32]; + uint8_t V[32]; + uint8_t Y[16]; + } cmacInput= {0,0,0}; + memcpy(cmacInput.U,U,32); + memcpy(cmacInput.V,V,32); + memcpy(cmacInput.Y,Y,16); + uint8_t intermediate[16]; + AES_CMAC(X,(uint8_t*)&cmacInput,sizeof(CmacInput),intermediate); + memcpy(out,&intermediate[12],4); + return 1; +} +void BluetoothCryptoToolbox::testg2(){ + uint8_t U[32] = {0x20,0xb0,0x03,0xd2,0xf2,0x97,0xbe,0x2c,0x5e,0x2c,0x83,0xa7,0xe9,0xf9,0xa5,0xb9,0xef,0xf4,0x91,0x11,0xac,0xf4,0xfd,0xdb,0xcc,0x03,0x01,0x48,0x0e,0x35,0x9d,0xe6}; + uint8_t V[32] = {0x55,0x18,0x8b,0x3d,0x32,0xf6,0xbb,0x9a,0x90,0x0a,0xfc,0xfb,0xee,0xd4,0xe7,0x2a,0x59,0xcb,0x9a,0xc2,0xf1,0x9d,0x7c,0xfb,0x6b,0x4f,0xdd,0x49,0xf4,0x7f,0xc5,0xfd}; + uint8_t X[16] = {0xd5,0xcb,0x84,0x54,0xd1,0x77,0x73,0x3e,0xff,0xff,0xb2,0xec,0x71,0x2b,0xae,0xab}; + uint8_t Y[16] = {0xa6,0xe8,0xe7,0xcc,0x25,0xa7,0x5f,0x6e,0x21,0x65,0x83,0xf7,0xff,0x3d,0xc4,0xcf}; + uint8_t AES[16] = {0x15,0x36,0xd1,0x8d,0xe3,0xd2,0x0d,0xf9,0x9b,0x70,0x44,0xc1,0x2f,0x9e,0xd5,0xba}; + uint8_t out[4]; + + + uint32_t expected = 0; + g2(U,V,X,Y,out); + uint32_t result = 0; + for(int i=0; i<4; i++) result += out[i] << 8*i; + + Serial.print("Expected : "); + Serial.println(expected); + Serial.print("Result : "); + Serial.println(result); + Serial.println(); + +} void BluetoothCryptoToolbox::AES_CMAC ( unsigned char *key, unsigned char *input, int length, unsigned char *mac ) diff --git a/src/utility/btct.h b/src/utility/btct.h index 43c0b0d6..08f8f192 100644 --- a/src/utility/btct.h +++ b/src/utility/btct.h @@ -13,11 +13,13 @@ class BluetoothCryptoToolbox{ int f5(uint8_t DHKey[],uint8_t N_master[], uint8_t N_slave[], uint8_t BD_ADDR_master[], uint8_t BD_ADDR_slave[], uint8_t MacKey[], uint8_t LTK[]); int f6(uint8_t W[], uint8_t N1[],uint8_t N2[],uint8_t R[], uint8_t IOCap[], uint8_t A1[], uint8_t A2[], uint8_t Ex[]); + int g2(uint8_t U[], uint8_t V[], uint8_t X[], uint8_t Y[], uint8_t out[4]); int ah(uint8_t k[16], uint8_t r[3], uint8_t result[3]); void test(); void testF5(); void testF6(); void testAh(); + void testg2(); private: int AES_128(uint8_t key[], uint8_t data_in[], uint8_t data_out[]); void leftshift_onebit(unsigned char *input,unsigned char *output); From 6b08d21480d00d2842f6641cf06d7aa48a7ec219 Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Thu, 7 Jan 2021 10:48:26 +0000 Subject: [PATCH 12/13] Updated example sketch --- .../EncryptedBatteryMonitor.ino | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino index fd40f5ee..dd3afbf6 100644 --- a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino +++ b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino @@ -18,7 +18,7 @@ #include -#define PAIR_BUTTON D3 // button for pairing +#define PAIR_BUTTON 3 // button for pairing #define PAIR_LED 24 // LED used to signal pairing #define PAIR_LED_ON LOW // Blue LED on Nano BLE has inverted logic #define PAIR_INTERVAL 30000 // interval for pairing after button press in ms @@ -42,6 +42,7 @@ int oldBatteryLevel = 0; // last battery level reading from analog input unsigned long previousMillis = 0; // last time the battery level was checked, in ms unsigned long pairingStarted = 0; // pairing start time when button is pressed bool wasConnected = 0; +bool acceptOrReject = true; void setup() { Serial.begin(9600); // initialize serial communication @@ -53,6 +54,30 @@ void setup() { Serial.println("Serial connected"); + + // Callback function with confirmation code when new device is pairing. + BLE.setDisplayCode([](uint32_t confirmCode){ + Serial.println("New device pairing request."); + Serial.print("Confirm code matches pairing device: "); + char code[6]; + sprintf(code, "%06d", confirmCode); + Serial.println(code); + }); + + // Callback to allow accepting or rejecting pairing + BLE.setBinaryConfirmPairing([&acceptOrReject](){ + Serial.print("Should we confirm pairing? "); + delay(5000); + if(acceptOrReject){ + acceptOrReject = false; + Serial.println("yes"); + return true; + }else{ + acceptOrReject = true; + Serial.println("no"); + return false; + } + }); // IRKs are keys that identify the true owner of a random mac address. // Add IRKs of devices you are bonded with. From 3dd53d1ba67450a26a80ec6768667a662d157496 Mon Sep 17 00:00:00 2001 From: unknownconstant Date: Sun, 24 Jan 2021 10:03:17 +0000 Subject: [PATCH 13/13] Spellcheck fixes --- .../EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino | 2 +- src/local/BLELocalDevice.cpp | 2 +- src/utility/ATT.cpp | 2 +- src/utility/HCI.cpp | 4 ++-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino index dd3afbf6..9f9d453b 100644 --- a/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino +++ b/examples/Peripheral/EncryptedBatteryMonitor/EncryptedBatteryMonitor.ino @@ -116,7 +116,7 @@ void setup() { // The LTK is the secret key which is used to encrypt bluetooth traffic BLE.setGetLTK([](uint8_t* address, uint8_t* LTK){ // address is input - Serial.print("Recieved request for address: "); + Serial.print("Received request for address: "); btct.printBytes(address,6); // Set these to the MAC and LTK of your devices after bonding. diff --git a/src/local/BLELocalDevice.cpp b/src/local/BLELocalDevice.cpp index d83c1cb3..8c1758b0 100644 --- a/src/local/BLELocalDevice.cpp +++ b/src/local/BLELocalDevice.cpp @@ -123,7 +123,7 @@ int BLELocalDevice::begin() /// The HCI should allow automatic address resolution. - // // If we have callbacks to rememember bonded devices: + // // If we have callbacks to remember bonded devices: // if(HCI._getIRKs!=0){ // uint8_t nIRKs = 0; // uint8_t** BADDR_Type = new uint8_t*; diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index 91ffeed0..f6ab392f 100644 --- a/src/utility/ATT.cpp +++ b/src/utility/ATT.cpp @@ -602,7 +602,7 @@ bool ATTClass::handleNotify(uint16_t handle, const uint8_t* value, int length) memcpy(¬ification[notificationLength], value, length); notificationLength += length; - /// TODO: Set encyption requirement on notify. + /// TODO: Set encryption requirement on notify. HCI.sendAclPkt(_peers[i].connectionHandle, ATT_CID, notificationLength, notification); numNotifications++; diff --git a/src/utility/HCI.cpp b/src/utility/HCI.cpp index 82303ed3..96948b85 100644 --- a/src/utility/HCI.cpp +++ b/src/utility/HCI.cpp @@ -841,7 +841,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) if((ATT.getPeerEncryption(encryptionChange->connectionHandle)&PEER_ENCRYPTION::PAIRING_REQUEST)>0){ if(ATT.localKeyDistribution.EncKey()){ #ifdef _BLE_TRACE_ - Serial.println("Enc key set but sould be ignored"); + Serial.println("Enc key set but should be ignored"); #endif }else{ #ifdef _BLE_TRACE_ @@ -1364,7 +1364,7 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[]) if((encryption & PEER_ENCRYPTION::RECEIVED_DH_CHECK) > 0){ #ifdef _BLE_TRACE_ - Serial.println("Recieved DHKey check already so calculate f5, f6 now."); + Serial.println("Received DHKey check already so calculate f5, f6 now."); #endif L2CAPSignaling.smCalculateLTKandConfirm(connectionHandle, HCI.remoteDHKeyCheckBuffer);