Merge pull request #153 from andresag01/update_adv_payload
Modify functions that manipulate adv payload
This commit is contained in:
commit
3036b058fe
15
ble/Gap.h
15
ble/Gap.h
|
@ -604,6 +604,16 @@ public:
|
|||
* @param type The type describing the variable length data.
|
||||
* @param data Data bytes.
|
||||
* @param len Length of data.
|
||||
*
|
||||
* @return BLE_ERROR_NONE if the advertisement payload was updated based on
|
||||
* matching AD type; otherwise, an appropriate error.
|
||||
*
|
||||
* @note When the specified AD type is INCOMPLETE_LIST_16BIT_SERVICE_IDS,
|
||||
* COMPLETE_LIST_16BIT_SERVICE_IDS, INCOMPLETE_LIST_32BIT_SERVICE_IDS,
|
||||
* COMPLETE_LIST_32BIT_SERVICE_IDS, INCOMPLETE_LIST_128BIT_SERVICE_IDS,
|
||||
* COMPLETE_LIST_128BIT_SERVICE_IDS or LIST_128BIT_SOLICITATION_IDS the
|
||||
* supplied value is appended to the values previously added to the
|
||||
* payload.
|
||||
*/
|
||||
ble_error_t accumulateAdvertisingPayload(GapAdvertisingData::DataType type, const uint8_t *data, uint8_t len) {
|
||||
if (type == GapAdvertisingData::COMPLETE_LOCAL_NAME) {
|
||||
|
@ -620,8 +630,7 @@ public:
|
|||
|
||||
/**
|
||||
* Update a particular ADV field in the advertising payload (based on
|
||||
* matching type and length). Note: the length of the new data must be the
|
||||
* same as the old one.
|
||||
* matching type).
|
||||
*
|
||||
* @param[in] type The ADV type field describing the variable length data.
|
||||
* @param[in] data Data bytes.
|
||||
|
@ -630,7 +639,7 @@ public:
|
|||
* @note: If advertisements are enabled, then the update will take effect immediately.
|
||||
*
|
||||
* @return BLE_ERROR_NONE if the advertisement payload was updated based on
|
||||
* a <type, len> match; otherwise, an appropriate error.
|
||||
* matching AD type; otherwise, an appropriate error.
|
||||
*/
|
||||
ble_error_t updateAdvertisingPayload(GapAdvertisingData::DataType type, const uint8_t *data, uint8_t len) {
|
||||
if (type == GapAdvertisingData::COMPLETE_LOCAL_NAME) {
|
||||
|
|
|
@ -202,149 +202,63 @@ public:
|
|||
|
||||
/**
|
||||
* Adds advertising data based on the specified AD type (see DataType).
|
||||
*
|
||||
* @param advDataType The Advertising 'DataType' to add.
|
||||
* @param payload Pointer to the payload contents.
|
||||
* @param len Size of the payload in bytes.
|
||||
*
|
||||
* @return BLE_ERROR_BUFFER_OVERFLOW if the specified data would cause the
|
||||
* advertising buffer to overflow, else BLE_ERROR_NONE.
|
||||
*/
|
||||
ble_error_t addData(DataType advDataType, const uint8_t *payload, uint8_t len)
|
||||
{
|
||||
ble_error_t result = 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);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Update a particular ADV field in the advertising payload (based on
|
||||
* matching type and length). Note: the length of the new data must be the
|
||||
* same as the old one.
|
||||
* If the supplied AD type is already present in the advertising
|
||||
* payload, then the value is updated.
|
||||
*
|
||||
* @param[in] advDataType The Advertising 'DataType' to add.
|
||||
* @param[in] payload Pointer to the payload contents.
|
||||
* @param[in] len Size of the payload in bytes.
|
||||
*
|
||||
* @return BLE_ERROR_UNSPECIFIED if the specified field is not found, else
|
||||
* BLE_ERROR_NONE.
|
||||
* @return BLE_ERROR_BUFFER_OVERFLOW if the new value causes the
|
||||
* advertising buffer to overflow. BLE_ERROR_NONE is returned
|
||||
* on success.
|
||||
*
|
||||
* @note When the specified AD type is INCOMPLETE_LIST_16BIT_SERVICE_IDS,
|
||||
* COMPLETE_LIST_16BIT_SERVICE_IDS, INCOMPLETE_LIST_32BIT_SERVICE_IDS,
|
||||
* COMPLETE_LIST_32BIT_SERVICE_IDS, INCOMPLETE_LIST_128BIT_SERVICE_IDS,
|
||||
* COMPLETE_LIST_128BIT_SERVICE_IDS or LIST_128BIT_SOLICITATION_IDS the
|
||||
* supplied value is appended to the values previously added to the
|
||||
* payload.
|
||||
*/
|
||||
ble_error_t addData(DataType_t advDataType, const uint8_t *payload, uint8_t len)
|
||||
{
|
||||
// find field
|
||||
uint8_t* field = findField(advDataType);
|
||||
|
||||
if (field) {
|
||||
// Field type already exist, either add to field or replace
|
||||
return addField(advDataType, payload, len, field);
|
||||
} else {
|
||||
// field doesn't exists, insert new
|
||||
return appendField(advDataType, payload, len);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update a particular ADV field in the advertising payload (based on
|
||||
* matching type).
|
||||
*
|
||||
* @param[in] advDataType The Advertising 'DataType' to add.
|
||||
* @param[in] payload Pointer to the payload contents.
|
||||
* @param[in] len Size of the payload in bytes.
|
||||
*
|
||||
* @return BLE_ERROR_UNSPECIFIED if the specified field is not found,
|
||||
* BLE_ERROR_BUFFER_OVERFLOW if the new value causes the
|
||||
* advertising buffer to overflow. BLE_ERROR_NONE is returned
|
||||
* on success.
|
||||
*/
|
||||
ble_error_t updateData(DataType_t advDataType, const uint8_t *payload, uint8_t len)
|
||||
{
|
||||
if ((payload == NULL) || (len == 0)) {
|
||||
return BLE_ERROR_INVALID_PARAM;
|
||||
// find field
|
||||
uint8_t* field = findField(advDataType);
|
||||
|
||||
if (field) {
|
||||
// Field type already exist, replace field contents
|
||||
return updateField(advDataType, payload, len, field);
|
||||
} else {
|
||||
// field doesn't exists, return an error
|
||||
return BLE_ERROR_UNSPECIFIED;
|
||||
}
|
||||
|
||||
/* A local struct to describe an ADV field. This definition comes from the Bluetooth Core Spec. (v4.2) Part C, Section 11. */
|
||||
struct ADVField_t {
|
||||
uint8_t len; /* Describes the length (in bytes) of the following type and bytes. */
|
||||
uint8_t type; /* Should have the same representation of DataType_t (above). */
|
||||
uint8_t bytes[0]; /* A placeholder for variable length data. */
|
||||
};
|
||||
|
||||
/* Iterate over the adv fields looking for the first match. */
|
||||
uint8_t byteIndex = 0;
|
||||
while (byteIndex < _payloadLen) {
|
||||
ADVField_t *currentADV = (ADVField_t *)&_payload[byteIndex];
|
||||
if ((currentADV->len == (len + 1)) && /* Incoming len only describes the payload, whereas ADV->len describes [type + payload]. */
|
||||
(currentADV->type == advDataType)) {
|
||||
memcpy(currentADV->bytes, payload, len);
|
||||
return BLE_ERROR_NONE;
|
||||
}
|
||||
|
||||
byteIndex += (currentADV->len + 1); /* Advance by len+1; '+1' is needed to span the len field itself. */
|
||||
}
|
||||
|
||||
return BLE_ERROR_UNSPECIFIED;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -475,6 +389,107 @@ private:
|
|||
return NULL;
|
||||
}
|
||||
|
||||
/**
|
||||
* Given the a pointer to a field in the advertising payload it replaces
|
||||
* the existing data in the field with the supplied data.
|
||||
*
|
||||
* When the specified AD type is INCOMPLETE_LIST_16BIT_SERVICE_IDS,
|
||||
* COMPLETE_LIST_16BIT_SERVICE_IDS, INCOMPLETE_LIST_32BIT_SERVICE_IDS,
|
||||
* COMPLETE_LIST_32BIT_SERVICE_IDS, INCOMPLETE_LIST_128BIT_SERVICE_IDS,
|
||||
* COMPLETE_LIST_128BIT_SERVICE_IDS or LIST_128BIT_SOLICITATION_IDS the
|
||||
* supplied value is appended to the values previously added to the
|
||||
* payload.
|
||||
*
|
||||
* Returns BLE_ERROR_NONE on success.
|
||||
*/
|
||||
ble_error_t addField(DataType_t advDataType, const uint8_t *payload, uint8_t len, uint8_t* field)
|
||||
{
|
||||
ble_error_t result = BLE_ERROR_BUFFER_OVERFLOW;
|
||||
|
||||
switch(advDataType) {
|
||||
// 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;
|
||||
}
|
||||
// These fields will be overwritten with the new value
|
||||
default: {
|
||||
result = updateField(advDataType, payload, len, field);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Given the a pointer to a field in the advertising payload it replaces
|
||||
* the existing data in the field with the supplied data.
|
||||
* Returns BLE_ERROR_NONE on success.
|
||||
*/
|
||||
ble_error_t updateField(DataType_t advDataType, const uint8_t *payload, uint8_t len, uint8_t* field)
|
||||
{
|
||||
ble_error_t result = BLE_ERROR_BUFFER_OVERFLOW;
|
||||
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];
|
||||
}
|
||||
|
||||
result = BLE_ERROR_NONE;
|
||||
} 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);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
uint8_t _payload[GAP_ADVERTISING_DATA_MAX_PAYLOAD];
|
||||
uint8_t _payloadLen;
|
||||
uint16_t _appearance;
|
||||
|
|
Loading…
Reference in New Issue