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 0ca0220f..df11e971 100644 --- a/src/local/BLELocalDevice.cpp +++ b/src/local/BLELocalDevice.cpp @@ -108,6 +108,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 9ebb0bc2..bcca86a9 100644 --- a/src/local/BLELocalDevice.h +++ b/src/local/BLELocalDevice.h @@ -81,6 +81,8 @@ class BLELocalDevice { virtual void debug(Stream& stream); virtual void noDebug(); + uint8_t BDaddress[6]; + protected: virtual BLEAdvertisingData& getAdvertisingData(); virtual BLEAdvertisingData& getScanResponseData(); diff --git a/src/utility/ATT.cpp b/src/utility/ATT.cpp index a1028f76..4c755615 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; @@ -808,6 +835,14 @@ void ATTClass::readByGroupReq(uint16_t connectionHandle, uint16_t mtu, uint8_t d uint16_t endHandle; uint16_t uuid; } *readByGroupReq = (ReadByGroupReq*)data; +#ifdef _BLE_TRACE_ + Serial.print("readByGroupReq: start: 0x"); + Serial.println(readByGroupReq->startHandle,HEX); + Serial.print("readByGroupReq: end: 0x"); + Serial.println(readByGroupReq->endHandle,HEX); + Serial.print("readByGroupReq: UUID: 0x"); + Serial.println(readByGroupReq->uuid,HEX); +#endif if (dlen != sizeof(ReadByGroupReq) || (readByGroupReq->uuid != BLETypeService && readByGroupReq->uuid != 0x2801)) { sendError(connectionHandle, ATT_OP_READ_BY_GROUP_REQ, readByGroupReq->startHandle, ATT_ECODE_UNSUPP_GRP_TYPE); @@ -820,7 +855,10 @@ void ATTClass::readByGroupReq(uint16_t connectionHandle, uint16_t mtu, uint8_t d response[0] = ATT_OP_READ_BY_GROUP_RESP; response[1] = 0x00; responseLength = 2; - +#ifdef _BLE_TRACE_ + Serial.print("readByGroupReq: attrcount: "); + Serial.println(GATT.attributeCount()); +#endif for (uint16_t i = (readByGroupReq->startHandle - 1); i < GATT.attributeCount() && i <= (readByGroupReq->endHandle - 1); i++) { BLELocalAttribute* attribute = GATT.attribute(i); @@ -907,6 +945,8 @@ void ATTClass::readOrReadBlobReq(uint16_t connectionHandle, uint16_t mtu, uint8_ return; } } + /// if auth error, hold the response in a buffer. + bool holdResponse = false; uint16_t handle = *(uint16_t*)data; uint16_t offset = (opcode == ATT_OP_READ_REQ) ? 0 : *(uint16_t*)&data[sizeof(handle)]; @@ -963,6 +1003,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(); @@ -995,8 +1040,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[]) @@ -1688,7 +1737,85 @@ 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 6d6b69ee..0b9c8302 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(); @@ -36,12 +60,14 @@ class HCIClass { virtual int reset(); virtual int readLocalVersion(uint8_t& hciVer, uint16_t& hciRev, uint8_t& lmpVer, uint16_t& manufacturer, uint16_t& lmpSubVer); + virtual int readBdAddr(uint8_t addr[6]); + virtual int readBdAddr(); virtual int readRssi(uint16_t handle); virtual int setEventMask(uint64_t eventMask); - + virtual int setLeEventMask(uint64_t leEventMask); virtual int readLeBufferSize(uint16_t& pktLen, uint8_t& maxPkt); virtual int leSetRandomAddress(uint8_t addr[6]); virtual int leSetAdvertisingParameters(uint16_t minInterval, uint16_t maxInterval, @@ -62,7 +88,7 @@ class HCIClass { virtual int leConnUpdate(uint16_t handle, uint16_t minInterval, uint16_t maxInterval, uint16_t latency, uint16_t supervisionTimeout); virtual int leCancelConn(); - + virtual int leEncrypt(uint8_t* Key, uint8_t* plaintext, uint8_t* status, uint8_t* ciphertext); virtual int sendAclPkt(uint16_t handle, uint8_t cid, uint8_t plen, void* data); @@ -71,8 +97,15 @@ class HCIClass { virtual void debug(Stream& stream); virtual void noDebug(); -private: + // 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 Na[16]; + uint8_t Nb[16]; + uint8_t DHKey[32]; + uint8_t localAddr[6]; + uint8_t LTK[16]; +private: virtual void handleAclDataPkt(uint8_t plen, uint8_t pdata[]); virtual void handleNumCompPkts(uint16_t handle, uint16_t numPkts); diff --git a/src/utility/L2CAPSignaling.cpp b/src/utility/L2CAPSignaling.cpp index 3cca56cc..3779eb29 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 @@ -108,6 +109,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 { virtual void handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); + virtual void handleSecurityData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]); + virtual void removeConnection(uint8_t handle, uint16_t reason); virtual void setConnectionInterval(uint16_t minInterval, uint16_t maxInterval); 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