/*

This file is part of the ArduinoBLE library.

Copyright (c) 2018 Arduino SA. All rights reserved.

This library is free software; you can redistribute it and/or

modify it under the terms of the GNU Lesser General Public

License as published by the Free Software Foundation; either

version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU

Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public

License along with this library; if not, write to the Free Software

Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA

*/

#include <Arduino.h>

#include "HCI.h"

#include "GATT.h"

#include "local/BLELocalAttribute.h"

#include "local/BLELocalCharacteristic.h"

#include "local/BLELocalDescriptor.h"

#include "local/BLELocalService.h"

#include "remote/BLERemoteDevice.h"

#include "remote/BLERemoteService.h"

#include "BLEProperty.h"

#include "ATT.h"

#define ATT_OP_ERROR 0x01

#define ATT_OP_MTU_REQ 0x02

#define ATT_OP_MTU_RESP 0x03

#define ATT_OP_FIND_INFO_REQ 0x04

#define ATT_OP_FIND_INFO_RESP 0x05

#define ATT_OP_FIND_BY_TYPE_REQ 0x06

#define ATT_OP_FIND_BY_TYPE_RESP 0x07

#define ATT_OP_READ_BY_TYPE_REQ 0x08

#define ATT_OP_READ_BY_TYPE_RESP 0x09

#define ATT_OP_READ_REQ 0x0a

#define ATT_OP_READ_RESP 0x0b

#define ATT_OP_READ_BLOB_REQ 0x0c

#define ATT_OP_READ_BLOB_RESP 0x0d

#define ATT_OP_READ_MULTI_REQ 0x0e

#define ATT_OP_READ_MULTI_RESP 0x0f

#define ATT_OP_READ_BY_GROUP_REQ 0x10

#define ATT_OP_READ_BY_GROUP_RESP 0x11

#define ATT_OP_WRITE_REQ 0x12

#define ATT_OP_WRITE_RESP 0x13

#define ATT_OP_WRITE_CMD 0x52

#define ATT_OP_PREP_WRITE_REQ 0x16

#define ATT_OP_PREP_WRITE_RESP 0x17

#define ATT_OP_EXEC_WRITE_REQ 0x18

#define ATT_OP_EXEC_WRITE_RESP 0x19

#define ATT_OP_HANDLE_NOTIFY 0x1b

#define ATT_OP_HANDLE_IND 0x1d

#define ATT_OP_HANDLE_CNF 0x1e

#define ATT_OP_SIGNED_WRITE_CMD 0xd2

#define ATT_ECODE_INVALID_HANDLE 0x01

#define ATT_ECODE_READ_NOT_PERM 0x02

#define ATT_ECODE_WRITE_NOT_PERM 0x03

#define ATT_ECODE_INVALID_PDU 0x04

#define ATT_ECODE_AUTHENTICATION 0x05

#define ATT_ECODE_REQ_NOT_SUPP 0x06

#define ATT_ECODE_INVALID_OFFSET 0x07

#define ATT_ECODE_AUTHORIZATION 0x08

#define ATT_ECODE_PREP_QUEUE_FULL 0x09

#define ATT_ECODE_ATTR_NOT_FOUND 0x0a

#define ATT_ECODE_ATTR_NOT_LONG 0x0b

#define ATT_ECODE_INSUFF_ENCR_KEY_SIZE 0x0c

#define ATT_ECODE_INVAL_ATTR_VALUE_LEN 0x0d

#define ATT_ECODE_UNLIKELY 0x0e

#define ATT_ECODE_INSUFF_ENC 0x0f

#define ATT_ECODE_UNSUPP_GRP_TYPE 0x10

#define ATT_ECODE_INSUFF_RESOURCES 0x11

// #define _BLE_TRACE_

ATTClass::ATTClass() :

_maxMtu(23),

_timeout(5000),

_longWriteHandle(0x0000),

_longWriteValue(NULL),

_longWriteValueLength(0)

{

for (int i = 0; i < ATT_MAX_PEERS; i++) {

_peers[i].connectionHandle = 0xffff;

_peers[i].role = 0x00;

_peers[i].addressType = 0x00;

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));

}

ATTClass::~ATTClass()

{

if (_longWriteValue) {

free(_longWriteValue);

}

}

bool ATTClass::connect(uint8_t peerBdaddrType, uint8_t peerBdaddr[6])

{

if (HCI.leCreateConn(0x0060, 0x0030, 0x00, peerBdaddrType, peerBdaddr, 0x00,

0x0006, 0x000c, 0x0000, 0x00c8, 0x0004, 0x0006) != 0) {

return false;

}

bool isConnected = false;

for (unsigned long start = millis(); (millis() - start) < _timeout;) {

HCI.poll();

isConnected = connected(peerBdaddrType, peerBdaddr);

if (isConnected) {

break;

}

}

if (!isConnected) {

HCI.leCancelConn();

}

return isConnected;

}

bool ATTClass::disconnect(uint8_t peerBdaddrType, uint8_t peerBdaddr[6])

{

uint16_t connHandle = connectionHandle(peerBdaddrType, peerBdaddr);

if (connHandle == 0xffff) {

return false;

}

HCI.disconnect(connHandle);

for (unsigned long start = millis(); (millis() - start) < _timeout;) {

HCI.poll();

if (!connected(connHandle)) {

return true;

}

}

return false;

}

bool ATTClass::discoverAttributes(uint8_t peerBdaddrType, uint8_t peerBdaddr[6], const char* serviceUuidFilter)

{

uint16_t connHandle = connectionHandle(peerBdaddrType, peerBdaddr);

if (connHandle == 0xffff) {

return false;

}

// send MTU request

if (!exchangeMtu(connHandle)) {

return false;

}

// find the device entry for the peeer

BLERemoteDevice* device = NULL;

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == connHandle) {

if (_peers[i].device == NULL) {

_peers[i].device = new BLERemoteDevice();

}

device = _peers[i].device;

break;

}

}

if (device == NULL) {

return false;

}

if (serviceUuidFilter == NULL) {

// clear existing services

device->clearServices();

} else {

int serviceCount = device->serviceCount();

for (int i = 0; i < serviceCount; i++) {

BLERemoteService* service = device->service(i);

if (strcasecmp(service->uuid(), serviceUuidFilter) == 0) {

// found an existing service with same UUID

return true;

}

}

}

// discover services

if (!discoverServices(connHandle, device, serviceUuidFilter)) {

return false;

}

// discover characteristics

if (!discoverCharacteristics(connHandle, device)) {

return false;

}

// discover descriptors

if (!discoverDescriptors(connHandle, device)) {

return false;

}

return true;

}

void ATTClass::setMaxMtu(uint16_t maxMtu)

{

_maxMtu = maxMtu;

}

void ATTClass::setTimeout(unsigned long timeout)

{

_timeout = timeout;

}

void ATTClass::addConnection(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*/)

{

int peerIndex = -1;

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == 0xffff) {

peerIndex = i;

break;

}

}

if (peerIndex == -1) {

// bail, no space

return;

}

_peers[peerIndex].connectionHandle = handle;

_peers[peerIndex].role = role;

_peers[peerIndex].mtu = 23;

_peers[peerIndex].addressType = peerBdaddrType;

memcpy(_peers[peerIndex].address, peerBdaddr, sizeof(_peers[peerIndex].address));

uint8_t BDADDr[6];

for(int i=0; i<6; i++) BDADDr[5-i] = peerBdaddr[i];

if(HCI.tryResolveAddress(BDADDr,_peers[peerIndex].resolvedAddress)){

#ifdef _BLE_TRACE_

Serial.println("Found match.");

#endif

}else{

#ifdef _BLE_TRACE_

Serial.println("No matching MAC");

#endif

memset(&_peers[peerIndex].resolvedAddress, 0, 6);

}

if (_eventHandlers[BLEConnected]) {

_eventHandlers[BLEConnected](BLEDevice(peerBdaddrType, peerBdaddr));

}

}

void ATTClass::handleData(uint16_t connectionHandle, uint8_t dlen, uint8_t data[])

{

uint8_t opcode = data[0];

dlen--;

data++;

uint16_t mtu = this->mtu(connectionHandle);

#ifdef _BLE_TRACE_

Serial.print("data opcode: 0x");

Serial.println(opcode, HEX);

#endif

switch (opcode) {

case ATT_OP_ERROR:

#ifdef _BLE_TRACE_

Serial.println("[Info] data error");

// Serial.print("Error: ");

// btct.printBytes(data, dlen);

#endif

error(connectionHandle, dlen, data);

break;

case ATT_OP_MTU_REQ:

#ifdef _BLE_TRACE_

Serial.println("MTU");

#endif

mtuReq(connectionHandle, dlen, data);

break;

case ATT_OP_MTU_RESP:

mtuResp(connectionHandle, dlen, data);

break;

case ATT_OP_FIND_INFO_REQ:

#ifdef _BLE_TRACE_

Serial.println("Find info");

#endif

findInfoReq(connectionHandle, mtu, dlen, data);

break;

case ATT_OP_FIND_INFO_RESP:

findInfoResp(connectionHandle, dlen, data);

break;

case ATT_OP_FIND_BY_TYPE_REQ:

findByTypeReq(connectionHandle, mtu, dlen, data);

break;

case ATT_OP_READ_BY_TYPE_REQ:

#ifdef _BLE_TRACE_

Serial.println("By type");

#endif

readByTypeReq(connectionHandle, mtu, dlen, data);

break;

case ATT_OP_READ_BY_TYPE_RESP:

readByTypeResp(connectionHandle, dlen, data);

break;

case ATT_OP_READ_BY_GROUP_REQ:

readByGroupReq(connectionHandle, mtu, dlen, data);

break;

case ATT_OP_READ_BY_GROUP_RESP:

readByGroupResp(connectionHandle, dlen, data);

break;

case ATT_OP_READ_REQ:

case ATT_OP_READ_BLOB_REQ:

readOrReadBlobReq(connectionHandle, mtu, opcode, dlen, data);

break;

case ATT_OP_READ_RESP:

readResp(connectionHandle, dlen, data);

break;

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;

case ATT_OP_WRITE_RESP:

writeResp(connectionHandle, dlen, data);

break;

case ATT_OP_PREP_WRITE_REQ:

prepWriteReq(connectionHandle, mtu, dlen, data);

break;

case ATT_OP_EXEC_WRITE_REQ:

execWriteReq(connectionHandle, mtu, dlen, data);

break;

case ATT_OP_HANDLE_NOTIFY:

case ATT_OP_HANDLE_IND:

handleNotifyOrInd(connectionHandle, opcode, dlen, data);

break;

case ATT_OP_HANDLE_CNF:

handleCnf(connectionHandle, dlen, data);

break;

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;

}

}

void ATTClass::removeConnection(uint16_t handle, uint8_t /*reason*/)

{

int peerIndex = -1;

int peerCount = 0;

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == handle) {

peerIndex = i;

}

if (_peers[i].connectionHandle != 0xffff) {

peerCount++;

}

}

if (peerIndex == -1) {

// bail not found

return;

}

BLEDevice bleDevice(_peers[peerIndex].addressType, _peers[peerIndex].address);

if (peerCount == 1) {

// clear CCCD values on disconnect

for (uint16_t i = 0; i < GATT.attributeCount(); i++) {

BLELocalAttribute* attribute = GATT.attribute(i);

if (attribute->type() == BLETypeCharacteristic) {

BLELocalCharacteristic* characteristic = (BLELocalCharacteristic*)attribute;

characteristic->writeCccdValue(bleDevice, 0x0000);

}

}

_longWriteHandle = 0x0000;

_longWriteValueLength = 0;

}

if (_eventHandlers[BLEDisconnected]) {

_eventHandlers[BLEDisconnected](bleDevice);

}

_peers[peerIndex].connectionHandle = 0xffff;

_peers[peerIndex].role = 0x00;

_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;

}

_peers[peerIndex].device = NULL;

}

uint16_t ATTClass::connectionHandle(uint8_t addressType, const uint8_t address[6]) const

{

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].addressType == addressType && memcmp(_peers[i].address, address, 6) == 0) {

return _peers[i].connectionHandle;

}

}

return 0xffff;

}

BLERemoteDevice* ATTClass::device(uint8_t addressType, const uint8_t address[6]) const

{

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].addressType == addressType && memcmp(_peers[i].address, address, 6) == 0) {

return _peers[i].device;

}

}

return NULL;

}

bool ATTClass::connected() const

{

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle != 0xffff) {

return true;

}

}

return false;

}

bool ATTClass::connected(uint8_t addressType, const uint8_t address[6]) const

{

return (connectionHandle(addressType, address) != 0xffff);

}

bool ATTClass::connected(uint16_t handle) const

{

HCI.poll();

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == handle) {

return true;

}

}

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<ATT_MAX_PEERS; i++){

if((_peers[i].encryption & PEER_ENCRYPTION::ENCRYPTED_AES) > 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<ATT_MAX_PEERS; i++){

if(_peers[i].connectionHandle != handle){continue;}

return (_peers[i].encryption & PEER_ENCRYPTION::ENCRYPTED_AES) > 0;

}

return false; // unknown handle

}

uint16_t ATTClass::mtu(uint16_t handle) const

{

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == handle) {

return _peers[i].mtu;

}

}

return 23;

}

bool ATTClass::disconnect()

{

int numDisconnects = 0;

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == 0xffff) {

continue;

}

if (HCI.disconnect(_peers[i].connectionHandle) != 0) {

continue;

}

numDisconnects++;

_peers[i].connectionHandle = 0xffff;

_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) {

delete _peers[i].device;

}

_peers[i].device = NULL;

}

return (numDisconnects > 0);

}

BLEDevice ATTClass::central()

{

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == 0xffff || _peers[i].role != 0x01) {

continue;

}

return BLEDevice(_peers[i].addressType, _peers[i].address);

}

return BLEDevice();

}

bool ATTClass::handleNotify(uint16_t handle, const uint8_t* value, int length)

{

int numNotifications = 0;

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == 0xffff) {

continue;

}

uint8_t notification[_peers[i].mtu];

uint16_t notificationLength = 0;

notification[0] = ATT_OP_HANDLE_NOTIFY;

notificationLength++;

memcpy(&notification[1], &handle, sizeof(handle));

notificationLength += sizeof(handle);

length = min((uint16_t)(_peers[i].mtu - notificationLength), (uint16_t)length);

memcpy(&notification[notificationLength], value, length);

notificationLength += length;

/// TODO: Set encryption requirement on notify.

HCI.sendAclPkt(_peers[i].connectionHandle, ATT_CID, notificationLength, notification);

numNotifications++;

}

return (numNotifications > 0);

}

bool ATTClass::handleInd(uint16_t handle, const uint8_t* value, int length)

{

int numIndications = 0;

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == 0xffff) {

continue;

}

uint8_t indication[_peers[i].mtu];

uint16_t indicationLength = 0;

indication[0] = ATT_OP_HANDLE_IND;

indicationLength++;

memcpy(&indication[1], &handle, sizeof(handle));

indicationLength += sizeof(handle);

length = min((uint16_t)(_peers[i].mtu - indicationLength), (uint16_t)length);

memcpy(&indication[indicationLength], value, length);

indicationLength += length;

_cnf = false;

HCI.sendAclPkt(_peers[i].connectionHandle, ATT_CID, indicationLength, indication);

while (!_cnf) {

HCI.poll();

if (!connected(_peers[i].addressType, _peers[i].address)) {

break;

}

}

numIndications++;

}

return (numIndications > 0);

}

void ATTClass::error(uint16_t connectionHandle, uint8_t dlen, uint8_t data[])

{

if (dlen != 4) {

// drop

return;

}

struct __attribute__ ((packed)) AttError {

uint8_t opcode;

uint16_t handle;

uint8_t code;

} *attError = (AttError*)data;

if (_pendingResp.connectionHandle == connectionHandle && (_pendingResp.op - 1) == attError->opcode) {

_pendingResp.buffer[0] = ATT_OP_ERROR;

memcpy(&_pendingResp.buffer[1], data, dlen);

_pendingResp.length = dlen + 1;

}

}

void ATTClass::mtuReq(uint16_t connectionHandle, uint8_t dlen, uint8_t data[])

{

uint16_t mtu = *(uint16_t*)data;

if (dlen != 2) {

sendError(connectionHandle, ATT_OP_MTU_REQ, 0x0000, ATT_ECODE_INVALID_PDU);

return;

}

if (mtu > _maxMtu) {

mtu = _maxMtu;

}

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == connectionHandle) {

_peers[i].mtu = mtu;

break;

}

}

struct __attribute__ ((packed)) {

uint8_t op;

uint16_t mtu;

} mtuResp = { ATT_OP_MTU_RESP, mtu };

HCI.sendAclPkt(connectionHandle, ATT_CID, sizeof(mtuResp), &mtuResp);

}

int ATTClass::mtuReq(uint16_t connectionHandle, uint16_t mtu, uint8_t responseBuffer[])

{

struct __attribute__ ((packed)) {

uint8_t op;

uint16_t mtu;

} mtuReq = { ATT_OP_MTU_REQ, mtu };

return sendReq(connectionHandle, &mtuReq, sizeof(mtuReq), responseBuffer);

}

void ATTClass::mtuResp(uint16_t connectionHandle, uint8_t dlen, uint8_t data[])

{

uint16_t mtu = *(uint16_t*)data;

if (dlen != 2) {

return;

}

for (int i = 0; i < ATT_MAX_PEERS; i++) {

if (_peers[i].connectionHandle == connectionHandle) {

_peers[i].mtu = mtu;

break;

}

}

if (connectionHandle == _pendingResp.connectionHandle && _pendingResp.op == ATT_OP_MTU_RESP) {

_pendingResp.buffer[0] = ATT_OP_MTU_RESP;

memcpy(&_pendingResp.buffer[1], data, dlen);

_pendingResp.length = dlen + 1;

}

}

void ATTClass::findInfoReq(uint16_t connectionHandle, uint16_t mtu, uint8_t dlen, uint8_t data[])

{

struct __attribute__ ((packed)) FindInfoReq {

uint16_t startHandle;

uint16_t endHandle;

} *findInfoReq = (FindInfoReq*)data;

if (dlen != sizeof(FindInfoReq)) {

sendError(connectionHandle, ATT_OP_FIND_INFO_REQ, findInfoReq->startHandle, ATT_ECODE_INVALID_PDU);

return;

}

uint8_t response[mtu];

uint16_t responseLength;

response[0] = ATT_OP_FIND_INFO_RESP;

response[1] = 0x00;

responseLength = 2;

for (uint16_t i = (findInfoReq->startHandle - 1); i < GATT.attributeCount() && i <= (findInfoReq->endHandle - 1); i++) {

BLELocalAttribute* attribute = GATT.attribute(i);

uint16_t handle = (i + 1);

bool isValueHandle = (attribute->type() == BLETypeCharacteristic) && (((BLELocalCharacteristic*)attribute)->valueHandle() == handle);

bool isDescriptor = attribute->type() == BLETypeDescriptor;

int uuidLen = (isValueHandle || isDescriptor) ? attribute->uuidLength() : BLE_ATTRIBUTE_TYPE_SIZE;

int infoType = (uuidLen == 2) ? 0x01 : 0x02;

if (response[1] == 0) {

response[1] = infoType;

}

if (response[1] != infoType) {

// different type

break;

}

// add the handle

memcpy(&response[responseLength], &handle, sizeof(handle));

responseLength += sizeof(handle);

if (isValueHandle || isDescriptor) {

// add the UUID

memcpy(&response[responseLength], attribute->uuidData(), uuidLen);

responseLength += uuidLen;

} else {

// add the type

uint16_t type = attribute->type();

memcpy(&response[responseLength], &type, sizeof(type));

responseLength += sizeof(type);

}

if ((responseLength + (2 + uuidLen)) > mtu) {

break;

}

}

if (responseLength == 2) {

sendError(connectionHandle, ATT_OP_FIND_INFO_REQ, findInfoReq->startHandle, ATT_ECODE_ATTR_NOT_FOUND);

} else {

HCI.sendAclPkt(connectionHandle, ATT_CID, responseLength, response);

}

}

int ATTClass::findInfoReq(uint16_t connectionHandle, uint16_t startHandle, uint16_t endHandle, uint8_t responseBuffer[])

{

struct __attribute__ ((packed)) {

uint8_t op;

uint16_t startHandle;

uint16_t endHandle;

} findInfoReq = { ATT_OP_FIND_INFO_REQ, startHandle, endHandle };

return sendReq(connectionHandle, &findInfoReq, sizeof(findInfoReq), responseBuffer);

}

void ATTClass::findInfoResp(uint16_t connectionHandle, uint8_t dlen, uint8_t data[])

{

if (dlen < 2) {

return; // invalid, drop

}

if (connectionHandle == _pendingResp.connectionHandle && _pendingResp.op == ATT_OP_FIND_INFO_RESP) {

_pendingResp.buffer[0] = ATT_OP_FIND_INFO_RESP;

memcpy(&_pendingResp.buffer[1], data, dlen);

_pendingResp.length = dlen + 1;

}

}

void ATTClass::findByTypeReq(uint16_t connectionHandle, uint16_t mtu, uint8_t dlen, uint8_t data[])

{

struct __attribute__ ((packed)) FindByTypeReq {

uint16_t startHandle;

uint16_t endHandle;

uint16_t type;

} *findByTypeReq = (FindByTypeReq*)data;

if (dlen < sizeof(FindByTypeReq)) {

sendError(connectionHandle, ATT_OP_FIND_BY_TYPE_REQ, findByTypeReq->startHandle, ATT_ECODE_INVALID_PDU);

return;

}

uint16_t valueLength = dlen - sizeof(*findByTypeReq);

uint8_t* value = &data[sizeof(*findByTypeReq)];

uint8_t response[mtu];

uint16_t responseLength;

response[0] = ATT_OP_FIND_BY_TYPE_RESP;

responseLength = 1;

if (findByTypeReq->type == BLETypeService) {

for (uint16_t i = (findByTypeReq->startHandle - 1); i < GATT.attributeCount() && i <= (findByTypeReq->endHandle - 1); i++) {

BLELocalAttribute* attribute = GATT.attribute(i);

if ((attribute->type() == findByTypeReq->type) && (attribute->uuidLength() == valueLength) && memcmp(attribute->uuidData(), value, valueLength) == 0) {

BLELocalService* service = (BLELocalService*)attribute;

// add the start handle

uint16_t startHandle = service->startHandle();

memcpy(&response[responseLength], &startHandle, sizeof(startHandle));

responseLength += sizeof(startHandle);

// add the end handle

uint16_t endHandle = service->endHandle();

memcpy(&response[responseLength], &endHandle, sizeof(endHandle));

responseLength += sizeof(endHandle);

}

if ((responseLength + 4) > mtu) {

break;

}

}

}

if (responseLength == 1) {

sendError(connectionHandle, ATT_OP_FIND_BY_TYPE_REQ, findByTypeReq->startHandle, ATT_ECODE_ATTR_NOT_FOUND);

} else {

HCI.sendAclPkt(connectionHandle, ATT_CID, responseLength, response);

}

}

void ATTClass::readByGroupReq(uint16_t connectionHandle, uint16_t mtu, uint8_t dlen, uint8_t data[])

{

struct __attribute__ ((packed)) ReadByGroupReq {

uint16_t startHandle;

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);

return;

}

uint8_t response[mtu];

uint16_t responseLength;

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);

if (readByGroupReq->uuid != attribute->type()) {

// not the type

continue;

}

int uuidLen = attribute->uuidLength();

int infoSize = (uuidLen == 2) ? 6 : 20;

if (response[1] == 0) {

response[1] = infoSize;

}

if (response[1] != infoSize) {

// different size

break;

}

BLELocalService* service = (BLELocalService*)attribute;

// add the start handle

uint16_t startHandle = service->startHandle();

memcpy(&response[responseLength], &startHandle, sizeof(startHandle));

responseLength += sizeof(startHandle);

// add the end handle

uint16_t endHandle = service->endHandle();

memcpy(&response[responseLength], &endHandle, sizeof(endHandle));

responseLength += sizeof(endHandle);

// add the UUID

memcpy(&response[responseLength], service->uuidData(), uuidLen);

responseLength += uuidLen;

if ((responseLength + infoSize) > mtu) {

break;

}

}

if (responseLength == 2) {

sendError(connectionHandle, ATT_OP_READ_BY_GROUP_REQ, readByGroupReq->startHandle, ATT_ECODE_ATTR_NOT_FOUND);

} else {

HCI.sendAclPkt(connectionHandle, ATT_CID, responseLength, response);

}

}

int ATTClass::readByGroupReq(uint16_t connectionHandle, uint16_t startHandle, uint16_t endHandle, uint16_t uuid, uint8_t responseBuffer[])

{

struct __attribute__ ((packed)) {

uint8_t op;

uint16_t startHandle;

uint16_t endHandle;

uint16_t uuid;

} readByGroupReq = { ATT_OP_READ_BY_GROUP_REQ, startHandle, endHandle, uuid };

return sendReq(connectionHandle, &readByGroupReq, sizeof(readByGroupReq), responseBuffer);

}

void ATTClass::readByGroupResp(uint16_t connectionHandle, uint8_t dlen, uint8_t data[])

{

if (dlen < 2) {

return; // invalid, drop

}

if (connectionHandle == _pendingResp.connectionHandle && _pendingResp.op == ATT_OP_READ_BY_GROUP_RESP) {

_pendingResp.buffer[0] = ATT_OP_READ_BY_GROUP_RESP;

memcpy(&_pendingResp.buffer[1], data, dlen);

_pendingResp.length = dlen + 1;

}

}

void ATTClass::readOrReadBlobReq(uint16_t connectionHandle, uint16_t mtu, uint8_t opcode, uint8_t dlen, uint8_t data[])

{

if (opcode == ATT_OP_READ_REQ) {

if (dlen != sizeof(uint16_t)) {

sendError(connectionHandle, ATT_OP_READ_REQ, 0x0000, ATT_ECODE_INVALID_PDU);

return;

}

} else {

if (dlen != (sizeof(uint16_t) + sizeof(uint16_t))) {

sendError(connectionHandle, ATT_OP_READ_BLOB_REQ, 0x0000, ATT_ECODE_INVALID_PDU);

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)];

if ((uint16_t)(handle - 1) > GATT.attributeCount()) {

sendError(connectionHandle, opcode, handle, ATT_ECODE_ATTR_NOT_FOUND);

return;