Merge pull request #108 from marcuschangarm/advert

Changed GapAdvertisingData.addData to take current fields into account when adding data.
This commit is contained in:
Rohit Grover 2015-11-24 17:35:46 +00:00
commit b6216416a4
1 changed files with 162 additions and 34 deletions

View File

@ -89,22 +89,23 @@ public:
*/
/**********************************************************************/
enum DataType_t {
FLAGS = 0x01, /**< \ref *Flags. */
INCOMPLETE_LIST_16BIT_SERVICE_IDS = 0x02, /**< Incomplete list of 16-bit service IDs. */
COMPLETE_LIST_16BIT_SERVICE_IDS = 0x03, /**< Complete list of 16-bit service IDs. */
INCOMPLETE_LIST_32BIT_SERVICE_IDS = 0x04, /**< Incomplete list of 32-bit service IDs (not relevant for Bluetooth 4.0). */
COMPLETE_LIST_32BIT_SERVICE_IDS = 0x05, /**< Complete list of 32-bit service IDs (not relevant for Bluetooth 4.0). */
INCOMPLETE_LIST_128BIT_SERVICE_IDS = 0x06, /**< Incomplete list of 128-bit service IDs. */
COMPLETE_LIST_128BIT_SERVICE_IDS = 0x07, /**< Complete list of 128-bit service IDs. */
SHORTENED_LOCAL_NAME = 0x08, /**< Shortened local name. */
COMPLETE_LOCAL_NAME = 0x09, /**< Complete local name. */
TX_POWER_LEVEL = 0x0A, /**< TX power level (in dBm). */
DEVICE_ID = 0x10, /**< Device ID. */
SLAVE_CONNECTION_INTERVAL_RANGE = 0x12, /**< Slave connection interval range. */
SERVICE_DATA = 0x16, /**< Service data. */
APPEARANCE = 0x19, /**< \ref Appearance. */
ADVERTISING_INTERVAL = 0x1A, /**< Advertising interval. */
MANUFACTURER_SPECIFIC_DATA = 0xFF /**< Manufacturer specific data. */
FLAGS = 0x01, /**< \ref *Flags */
INCOMPLETE_LIST_16BIT_SERVICE_IDS = 0x02, /**< Incomplete list of 16-bit Service IDs */
COMPLETE_LIST_16BIT_SERVICE_IDS = 0x03, /**< Complete list of 16-bit Service IDs */
INCOMPLETE_LIST_32BIT_SERVICE_IDS = 0x04, /**< Incomplete list of 32-bit Service IDs (not relevant for Bluetooth 4.0) */
COMPLETE_LIST_32BIT_SERVICE_IDS = 0x05, /**< Complete list of 32-bit Service IDs (not relevant for Bluetooth 4.0) */
INCOMPLETE_LIST_128BIT_SERVICE_IDS = 0x06, /**< Incomplete list of 128-bit Service IDs */
COMPLETE_LIST_128BIT_SERVICE_IDS = 0x07, /**< Complete list of 128-bit Service IDs */
SHORTENED_LOCAL_NAME = 0x08, /**< Shortened Local Name */
COMPLETE_LOCAL_NAME = 0x09, /**< Complete Local Name */
TX_POWER_LEVEL = 0x0A, /**< TX Power Level (in dBm) */
DEVICE_ID = 0x10, /**< Device ID */
SLAVE_CONNECTION_INTERVAL_RANGE = 0x12, /**< Slave Connection Interval Range */
LIST_128BIT_SOLICITATION_IDS = 0x15, /**< List of 128 bit service UUIDs the device is looking for */
SERVICE_DATA = 0x16, /**< Service Data */
APPEARANCE = 0x19, /**< \ref Appearance */
ADVERTISING_INTERVAL = 0x1A, /**< Advertising Interval */
MANUFACTURER_SPECIFIC_DATA = 0xFF /**< Manufacturer Specific Data */
};
typedef enum DataType_t DataType; /* Deprecated type alias. This may be dropped in a future release. */
@ -210,27 +211,98 @@ public:
*/
ble_error_t addData(DataType advDataType, const uint8_t *payload, uint8_t len)
{
/* To Do: Check if an AD type already exists and if the existing */
/* value is exclusive or not (flags and so on). */
ble_error_t result = BLE_ERROR_BUFFER_OVERFLOW;
/* Make sure we don't exceed the 31 byte payload limit. */
if (_payloadLen + len + 2 > GAP_ADVERTISING_DATA_MAX_PAYLOAD) {
return BLE_ERROR_BUFFER_OVERFLOW;
// find field
uint8_t* field = findField(advDataType);
// Field type already exist, either add to field or replace
if (field) {
switch(advDataType) {
// These fields will be overwritten with the new value
case FLAGS:
case SHORTENED_LOCAL_NAME:
case COMPLETE_LOCAL_NAME:
case TX_POWER_LEVEL:
case DEVICE_ID:
case SLAVE_CONNECTION_INTERVAL_RANGE:
case SERVICE_DATA:
case APPEARANCE:
case ADVERTISING_INTERVAL:
case MANUFACTURER_SPECIFIC_DATA: {
// current field length, with the type subtracted
uint8_t dataLength = field[0] - 1;
// new data has same length, do in-order replacement
if (len == dataLength) {
for (uint8_t idx = 0; idx < dataLength; idx++) {
field[2 + idx] = payload[idx];
}
} else {
// check if data fits
if ((_payloadLen - dataLength + len) <= GAP_ADVERTISING_DATA_MAX_PAYLOAD) {
// remove old field
while ((field + dataLength + 2) < &_payload[_payloadLen]) {
*field = field[dataLength + 2];
field++;
}
// reduce length
_payloadLen -= dataLength + 2;
// add new field
result = appendField(advDataType, payload, len);
}
}
break;
}
// These fields will have the new data appended if there is sufficient space
case INCOMPLETE_LIST_16BIT_SERVICE_IDS:
case COMPLETE_LIST_16BIT_SERVICE_IDS:
case INCOMPLETE_LIST_32BIT_SERVICE_IDS:
case COMPLETE_LIST_32BIT_SERVICE_IDS:
case INCOMPLETE_LIST_128BIT_SERVICE_IDS:
case COMPLETE_LIST_128BIT_SERVICE_IDS:
case LIST_128BIT_SOLICITATION_IDS: {
// check if data fits
if ((_payloadLen + len) <= GAP_ADVERTISING_DATA_MAX_PAYLOAD) {
// make room for new field by moving the remainder of the
// advertisement payload "to the right" starting after the
// TYPE field.
uint8_t* end = &_payload[_payloadLen];
while (&field[1] < end) {
end[len] = *end;
end--;
}
// insert new data
for (uint8_t idx = 0; idx < len; idx++) {
field[2 + idx] = payload[idx];
}
// increment lengths
field[0] += len;
_payloadLen += len;
result = BLE_ERROR_NONE;
}
break;
}
// Field exists but updating it is not supported. Abort operation.
default:
result = BLE_ERROR_NOT_IMPLEMENTED;
break;
}
} else {
// field doesn't exists, insert new
result = appendField(advDataType, payload, len);
}
/* Field length. */
memset(&_payload[_payloadLen], len + 1, 1);
_payloadLen++;
/* Field ID. */
memset(&_payload[_payloadLen], (uint8_t)advDataType, 1);
_payloadLen++;
/* Payload. */
memcpy(&_payload[_payloadLen], payload, len);
_payloadLen += len;
return BLE_ERROR_NONE;
return result;
}
/**
@ -345,7 +417,63 @@ public:
return (uint16_t)_appearance;
}
/**
* Search advertisement data for field.
* Returns pointer to the first element in the field if found, NULL otherwise.
* Where the first element is the length of the field.
*/
const uint8_t* findField(DataType_t type) const {
return findField(type);
}
private:
/**
* Append advertising data based on the specified AD type (see DataType)
*/
ble_error_t appendField(DataType advDataType, const uint8_t *payload, uint8_t len)
{
/* Make sure we don't exceed the 31 byte payload limit */
if (_payloadLen + len + 2 > GAP_ADVERTISING_DATA_MAX_PAYLOAD) {
return BLE_ERROR_BUFFER_OVERFLOW;
}
/* Field length. */
memset(&_payload[_payloadLen], len + 1, 1);
_payloadLen++;
/* Field ID. */
memset(&_payload[_payloadLen], (uint8_t)advDataType, 1);
_payloadLen++;
/* Payload. */
memcpy(&_payload[_payloadLen], payload, len);
_payloadLen += len;
return BLE_ERROR_NONE;
}
/**
* Search advertisement data for field.
* Returns pointer to the first element in the field if found, NULL otherwise.
* Where the first element is the length of the field.
*/
uint8_t* findField(DataType_t type) {
// scan through advertisement data
for (uint8_t idx = 0; idx < _payloadLen; ) {
uint8_t fieldType = _payload[idx + 1];
if (fieldType == type) {
return &_payload[idx];
}
// advance to next field
idx += _payload[idx] + 1;
}
// field not found
return NULL;
}
uint8_t _payload[GAP_ADVERTISING_DATA_MAX_PAYLOAD];
uint8_t _payloadLen;
uint16_t _appearance;