Skip to content

Commit

Permalink
Merge pull request #153 from andresag01/update_adv_payload
Browse files Browse the repository at this point in the history 
Modify functions that manipulate adv payload
  • Loading branch information
@pan-
pan- committed Dec 21, 2015
2 parents fa80684 + 484382e commit 3036b05
Show file tree
Hide file tree
Showing 2 changed files with 145 additions and 121 deletions.
15 changes: 12 additions & 3 deletions ble/Gap.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -604,6 +604,16 @@ class Gap {
* @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) {
Expand All @@ -620,8 +630,7 @@ class Gap {

/**
* 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.
Expand All @@ -630,7 +639,7 @@ class Gap {
* @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) {
Expand Down
251 changes: 133 additions & 118 deletions ble/GapAdvertisingData.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -202,149 +202,63 @@ class GapAdvertisingData

/**
* Adds advertising data based on the specified AD type (see DataType).
* If the supplied AD type is already present in the advertising
* payload, then the value is updated.
*
* @param advDataType The Advertising 'DataType' to add.
* @param payload Pointer to the payload contents.
* @param len Size of the payload in bytes.
* @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_BUFFER_OVERFLOW if the specified data would cause the
* advertising buffer to overflow, 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 advDataType, const uint8_t *payload, uint8_t len)
ble_error_t addData(DataType_t 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;
}
// Field type already exist, either add to field or replace
return addField(advDataType, payload, len, field);
} else {
// field doesn't exists, insert new
result = appendField(advDataType, payload, len);
return 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.
* 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, else
* BLE_ERROR_NONE.
* @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;
}

/* 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;
}
// find field
uint8_t* field = findField(advDataType);

byteIndex += (currentADV->len + 1); /* Advance by len+1; '+1' is needed to span the len field itself. */
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;
}

return BLE_ERROR_UNSPECIFIED;
}

/**
Expand Down Expand Up @@ -475,6 +389,107 @@ class GapAdvertisingData
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;
Expand Down

0 comments on commit 3036b05

Please sign in to comment.