Initial - encrypted read

Author: unknownconstant

src/ArduinoBLE.h

@@ -24,5 +24,6 @@
 #include "BLEProperty.h"
 #include "BLEStringCharacteristic.h"
 #include "BLETypedCharacteristics.h"
+#include "utility/btct.h"
 
 #endif

src/BLEProperty.h

@@ -17,6 +17,8 @@
   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */
 
+// #include <stdint.h>
+
 #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

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;

src/local/BLELocalDevice.h

@@ -74,6 +74,7 @@ class BLELocalDevice {
 
   void debug(Stream& stream);
   void noDebug();
+  uint8_t BDaddress[6];
 
 private:
 };

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<ATT_MAX_PEERS; i++){
+    if(_peers[i].connectionHandle != connectionHandle){
+      continue;
+    }
+    _peers[i].encryption = encryption;
+    return 1;
+  }
+  return 0;
+}
+// Set the IO capabilities for a peer
+int ATTClass::setPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[3]){
+  for(int i=0; i<ATT_MAX_PEERS; i++){
+    if(_peers[i].connectionHandle != connectionHandle){
+      continue;
+    }
+    memcpy(_peers[i].IOCap, IOCap, 3);
+    return 1;
+  }
+  return 0;
+}
+// Return the connection handle for the first peer that is requesting encryption
+uint16_t ATTClass::getPeerEncrptingConnectionHandle(){
+  for(int i=0; i<ATT_MAX_PEERS; i++){
+    if(_peers[i].encryption & PEER_ENCRYPTION::REQUESTED_ENCRYPTION > 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<ATT_MAX_PEERS; i++){
+    if(_peers[i].connectionHandle != connectionHandle){continue;}
+    return _peers[i].encryption;
+  }
+  return 0;
+}
+// Get the IOCapabilities for a peer
+int ATTClass::getPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[3]) {
+  for(int i=0; i<ATT_MAX_PEERS; i++){
+    if(_peers[i].connectionHandle != connectionHandle){continue;}
+    // return _peers[i].encryption;
+    memcpy(IOCap, _peers[i].IOCap, 3);
+  }
+  return 0;
+}
+// Get the BD_ADDR for a peer
+int ATTClass::getPeerAddr(uint16_t connectionHandle, uint8_t peerAddr[])
+{
+  for(int i=0; i<ATT_MAX_PEERS; i++)
+  {
+    if(_peers[i].connectionHandle != connectionHandle){continue;}
+    memcpy(peerAddr, _peers[i].address,6);
+    return 1;
+  }
+  return 0;
+}
+// Get the BD_ADDR for a peer in the format needed by f6 for pairing.
+int ATTClass::getPeerAddrWithType(uint16_t connectionHandle, uint8_t peerAddr[])
+{
+  for(int i=0; i<ATT_MAX_PEERS; i++)
+  {
+    if(_peers[i].connectionHandle != connectionHandle){continue;}
+    for(int k=0; k<6; k++){
+      peerAddr[6-k] = _peers[i].address[k];
+    }
+    if(_peers[i].addressType){
+      peerAddr[0] = 0x01;
+    }else{
+      peerAddr[0] = 0x00;
+    }
+    return 1;
+  }
+  return 0;
+}
 ATTClass ATT;

src/utility/ATT.h

@@ -25,6 +25,7 @@
 #include "BLEDevice.h"
 
 #define ATT_CID       0x0004
+#define BLE_CTL       0x0008
 
 #if defined(ARDUINO_PORTENTA_H7_M4) || defined(ARDUINO_PORTENTA_H7_M7)
 #define ATT_MAX_PEERS 7
@@ -34,6 +35,16 @@
 #define ATT_MAX_PEERS 3
 #endif
 
+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,
+  ENCRYPTED_AES         = 1 << 7
+};
+
 class BLERemoteDevice;
 
 class ATTClass {
@@ -76,7 +87,15 @@ class ATTClass {
   int readReq(uint16_t connectionHandle, uint16_t handle, uint8_t responseBuffer[]);
   int writeReq(uint16_t connectionHandle, uint16_t handle, const uint8_t* data, uint8_t dataLen, uint8_t responseBuffer[]);
   void writeCmd(uint16_t connectionHandle, uint16_t handle, const uint8_t* data, uint8_t dataLen);
-
+  int setPeerEncryption(uint16_t connectionHandle, uint8_t encryption);
+  uint8_t getPeerEncryption(uint16_t connectionHandle);
+  uint16_t getPeerEncrptingConnectionHandle();
+  int getPeerAddr(uint16_t connectionHandle, uint8_t peerAddr[]);
+  int getPeerAddrWithType(uint16_t connectionHandle, uint8_t peerAddr[]);
+  int setPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[]);
+  int getPeerIOCap(uint16_t connectionHandle, uint8_t IOCap[]);
+  uint8_t holdBuffer[64];
+  uint8_t holdBufferSize;
 private:
   void error(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]);
   void mtuReq(uint16_t connectionHandle, uint8_t dlen, uint8_t data[]);
@@ -119,6 +138,8 @@ class ATTClass {
     uint8_t address[6];
     uint16_t mtu;
     BLERemoteDevice* device;
+    uint8_t encryption;
+    uint8_t IOCap[3];
   } _peers[ATT_MAX_PEERS];
 
   volatile bool _cnf;