Junichi Katsu / Mbed 2 deprecated BLE_MPU6050_test6_challenge_sb

テスト用です。

Dependencies:   mbed

nRF51822_wallbot/nordic/ble/ble_advdata.cpp@0:8468a4403fea, 2016-10-11 (annotated)

Committer:
jksoft
Date:
Tue Oct 11 11:09:42 2016 +0000
Revision:
0:8468a4403fea
SB??ver;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jksoft 0:8468a4403fea 1 /* Copyright (c) 2012 Nordic Semiconductor. All Rights Reserved.
jksoft 0:8468a4403fea 2 *
jksoft 0:8468a4403fea 3 * The information contained herein is property of Nordic Semiconductor ASA.
jksoft 0:8468a4403fea 4 * Terms and conditions of usage are described in detail in NORDIC
jksoft 0:8468a4403fea 5 * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
jksoft 0:8468a4403fea 6 *
jksoft 0:8468a4403fea 7 * Licensees are granted free, non-transferable use of the information. NO
jksoft 0:8468a4403fea 8 * WARRANTY of ANY KIND is provided. This heading must NOT be removed from
jksoft 0:8468a4403fea 9 * the file.
jksoft 0:8468a4403fea 10 *
jksoft 0:8468a4403fea 11 */
jksoft 0:8468a4403fea 12
jksoft 0:8468a4403fea 13 #include "ble_advdata.h"
jksoft 0:8468a4403fea 14 #include "nordic_common.h"
jksoft 0:8468a4403fea 15 #include "nrf_error.h"
jksoft 0:8468a4403fea 16 #include "ble_gap.h"
jksoft 0:8468a4403fea 17 #include "ble_srv_common.h"
jksoft 0:8468a4403fea 18 #include "app_util.h"
jksoft 0:8468a4403fea 19
jksoft 0:8468a4403fea 20
jksoft 0:8468a4403fea 21 // Offset from where advertisement data other than flags information can start.
jksoft 0:8468a4403fea 22 #define ADV_FLAG_OFFSET 2
jksoft 0:8468a4403fea 23
jksoft 0:8468a4403fea 24 // Offset for Advertising Data.
jksoft 0:8468a4403fea 25 // Offset is 2 as each Advertising Data contain 1 octet of Adveritising Data Type and
jksoft 0:8468a4403fea 26 // one octet Advertising Data Length.
jksoft 0:8468a4403fea 27 #define ADV_DATA_OFFSET 2
jksoft 0:8468a4403fea 28
jksoft 0:8468a4403fea 29 // NOTE: For now, Security Manager TK Value and Security Manager Out of Band Flags (OOB) are omitted
jksoft 0:8468a4403fea 30 // from the advertising data.
jksoft 0:8468a4403fea 31
jksoft 0:8468a4403fea 32
jksoft 0:8468a4403fea 33 static uint32_t name_encode(const ble_advdata_t * p_advdata,
jksoft 0:8468a4403fea 34 uint8_t * p_encoded_data,
jksoft 0:8468a4403fea 35 uint8_t * p_len)
jksoft 0:8468a4403fea 36 {
jksoft 0:8468a4403fea 37 uint32_t err_code;
jksoft 0:8468a4403fea 38 uint16_t rem_adv_data_len;
jksoft 0:8468a4403fea 39 uint16_t actual_length;
jksoft 0:8468a4403fea 40 uint8_t adv_data_format;
jksoft 0:8468a4403fea 41 uint8_t adv_offset;
jksoft 0:8468a4403fea 42
jksoft 0:8468a4403fea 43 adv_offset = *p_len;
jksoft 0:8468a4403fea 44
jksoft 0:8468a4403fea 45
jksoft 0:8468a4403fea 46 // Check for buffer overflow.
jksoft 0:8468a4403fea 47 if ((adv_offset + ADV_DATA_OFFSET > BLE_GAP_ADV_MAX_SIZE) ||
jksoft 0:8468a4403fea 48 ((p_advdata->short_name_len + ADV_DATA_OFFSET) > BLE_GAP_ADV_MAX_SIZE))
jksoft 0:8468a4403fea 49 {
jksoft 0:8468a4403fea 50 return NRF_ERROR_DATA_SIZE;
jksoft 0:8468a4403fea 51 }
jksoft 0:8468a4403fea 52 actual_length = rem_adv_data_len = (BLE_GAP_ADV_MAX_SIZE - adv_offset - ADV_FLAG_OFFSET);
jksoft 0:8468a4403fea 53
jksoft 0:8468a4403fea 54 // Get GAP device name and length
jksoft 0:8468a4403fea 55 err_code = sd_ble_gap_device_name_get(&p_encoded_data[adv_offset + ADV_DATA_OFFSET],
jksoft 0:8468a4403fea 56 &actual_length);
jksoft 0:8468a4403fea 57 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 58 {
jksoft 0:8468a4403fea 59 return err_code;
jksoft 0:8468a4403fea 60 }
jksoft 0:8468a4403fea 61
jksoft 0:8468a4403fea 62 // Check if device internd to use short name and it can fit available data size.
jksoft 0:8468a4403fea 63 if ((p_advdata->name_type == BLE_ADVDATA_FULL_NAME) && (actual_length <= rem_adv_data_len))
jksoft 0:8468a4403fea 64 {
jksoft 0:8468a4403fea 65 // Complete device name can fit, setting Complete Name in Adv Data.
jksoft 0:8468a4403fea 66 adv_data_format = BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME;
jksoft 0:8468a4403fea 67 rem_adv_data_len = actual_length;
jksoft 0:8468a4403fea 68 }
jksoft 0:8468a4403fea 69 else
jksoft 0:8468a4403fea 70 {
jksoft 0:8468a4403fea 71 // Else short name needs to be used. Or application has requested use of short name.
jksoft 0:8468a4403fea 72 adv_data_format = BLE_GAP_AD_TYPE_SHORT_LOCAL_NAME;
jksoft 0:8468a4403fea 73
jksoft 0:8468a4403fea 74 // If application has set a preference on the short name size, it needs to be considered,
jksoft 0:8468a4403fea 75 // else fit what can be fit.
jksoft 0:8468a4403fea 76 if ((p_advdata->short_name_len != 0) && (p_advdata->short_name_len <= rem_adv_data_len))
jksoft 0:8468a4403fea 77 {
jksoft 0:8468a4403fea 78 // Short name fits available size.
jksoft 0:8468a4403fea 79 rem_adv_data_len = p_advdata->short_name_len;
jksoft 0:8468a4403fea 80 }
jksoft 0:8468a4403fea 81 // Else whatever can fit the data buffer will be packed.
jksoft 0:8468a4403fea 82 else
jksoft 0:8468a4403fea 83 {
jksoft 0:8468a4403fea 84 rem_adv_data_len = actual_length;
jksoft 0:8468a4403fea 85 }
jksoft 0:8468a4403fea 86 }
jksoft 0:8468a4403fea 87
jksoft 0:8468a4403fea 88 // Complete name field in encoded data.
jksoft 0:8468a4403fea 89 p_encoded_data[adv_offset++] = rem_adv_data_len + 1;
jksoft 0:8468a4403fea 90 p_encoded_data[adv_offset++] = adv_data_format;
jksoft 0:8468a4403fea 91 (*p_len) += (rem_adv_data_len + ADV_DATA_OFFSET);
jksoft 0:8468a4403fea 92
jksoft 0:8468a4403fea 93 return NRF_SUCCESS;
jksoft 0:8468a4403fea 94 }
jksoft 0:8468a4403fea 95
jksoft 0:8468a4403fea 96
jksoft 0:8468a4403fea 97 static uint32_t appearance_encode(uint8_t * p_encoded_data, uint8_t * p_len)
jksoft 0:8468a4403fea 98 {
jksoft 0:8468a4403fea 99 uint32_t err_code;
jksoft 0:8468a4403fea 100 uint16_t appearance;
jksoft 0:8468a4403fea 101
jksoft 0:8468a4403fea 102 // Check for buffer overflow.
jksoft 0:8468a4403fea 103 if ((*p_len) + 4 > BLE_GAP_ADV_MAX_SIZE)
jksoft 0:8468a4403fea 104 {
jksoft 0:8468a4403fea 105 return NRF_ERROR_DATA_SIZE;
jksoft 0:8468a4403fea 106 }
jksoft 0:8468a4403fea 107
jksoft 0:8468a4403fea 108 // Get GAP appearance field.
jksoft 0:8468a4403fea 109 err_code = sd_ble_gap_appearance_get(&appearance);
jksoft 0:8468a4403fea 110 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 111 {
jksoft 0:8468a4403fea 112 return err_code;
jksoft 0:8468a4403fea 113 }
jksoft 0:8468a4403fea 114
jksoft 0:8468a4403fea 115 // Encode Length, AD Type and Appearance.
jksoft 0:8468a4403fea 116 p_encoded_data[(*p_len)++] = 3;
jksoft 0:8468a4403fea 117 p_encoded_data[(*p_len)++] = BLE_GAP_AD_TYPE_APPEARANCE;
jksoft 0:8468a4403fea 118
jksoft 0:8468a4403fea 119 (*p_len) += uint16_encode(appearance, &p_encoded_data[*p_len]);
jksoft 0:8468a4403fea 120
jksoft 0:8468a4403fea 121 return NRF_SUCCESS;
jksoft 0:8468a4403fea 122 }
jksoft 0:8468a4403fea 123
jksoft 0:8468a4403fea 124
jksoft 0:8468a4403fea 125 static uint32_t uint8_array_encode(const uint8_array_t * p_uint8_array,
jksoft 0:8468a4403fea 126 uint8_t adv_type,
jksoft 0:8468a4403fea 127 uint8_t * p_encoded_data,
jksoft 0:8468a4403fea 128 uint8_t * p_len)
jksoft 0:8468a4403fea 129 {
jksoft 0:8468a4403fea 130 // Check parameter consistency.
jksoft 0:8468a4403fea 131 if (p_uint8_array->p_data == NULL)
jksoft 0:8468a4403fea 132 {
jksoft 0:8468a4403fea 133 return NRF_ERROR_INVALID_PARAM;
jksoft 0:8468a4403fea 134 }
jksoft 0:8468a4403fea 135
jksoft 0:8468a4403fea 136 // Check for buffer overflow.
jksoft 0:8468a4403fea 137 if ((*p_len) + ADV_DATA_OFFSET + p_uint8_array->size > BLE_GAP_ADV_MAX_SIZE)
jksoft 0:8468a4403fea 138 {
jksoft 0:8468a4403fea 139 return NRF_ERROR_DATA_SIZE;
jksoft 0:8468a4403fea 140 }
jksoft 0:8468a4403fea 141
jksoft 0:8468a4403fea 142 // Encode Length and AD Type.
jksoft 0:8468a4403fea 143 p_encoded_data[(*p_len)++] = 1 + p_uint8_array->size;
jksoft 0:8468a4403fea 144 p_encoded_data[(*p_len)++] = adv_type;
jksoft 0:8468a4403fea 145
jksoft 0:8468a4403fea 146 // Encode array.
jksoft 0:8468a4403fea 147 memcpy(&p_encoded_data[*p_len], p_uint8_array->p_data, p_uint8_array->size);
jksoft 0:8468a4403fea 148 (*p_len) += p_uint8_array->size;
jksoft 0:8468a4403fea 149
jksoft 0:8468a4403fea 150 return NRF_SUCCESS;
jksoft 0:8468a4403fea 151 }
jksoft 0:8468a4403fea 152
jksoft 0:8468a4403fea 153
jksoft 0:8468a4403fea 154 static uint32_t tx_power_level_encode(int8_t tx_power_level,
jksoft 0:8468a4403fea 155 uint8_t * p_encoded_data,
jksoft 0:8468a4403fea 156 uint8_t * p_len)
jksoft 0:8468a4403fea 157 {
jksoft 0:8468a4403fea 158 // Check for buffer overflow.
jksoft 0:8468a4403fea 159 if ((*p_len) + 3 > BLE_GAP_ADV_MAX_SIZE)
jksoft 0:8468a4403fea 160 {
jksoft 0:8468a4403fea 161 return NRF_ERROR_DATA_SIZE;
jksoft 0:8468a4403fea 162 }
jksoft 0:8468a4403fea 163
jksoft 0:8468a4403fea 164 // Encode TX Power Level.
jksoft 0:8468a4403fea 165 p_encoded_data[(*p_len)++] = 2;
jksoft 0:8468a4403fea 166 p_encoded_data[(*p_len)++] = BLE_GAP_AD_TYPE_TX_POWER_LEVEL;
jksoft 0:8468a4403fea 167 p_encoded_data[(*p_len)++] = (uint8_t)tx_power_level;
jksoft 0:8468a4403fea 168
jksoft 0:8468a4403fea 169 return NRF_SUCCESS;
jksoft 0:8468a4403fea 170 }
jksoft 0:8468a4403fea 171
jksoft 0:8468a4403fea 172
jksoft 0:8468a4403fea 173 static uint32_t uuid_list_sized_encode(const ble_advdata_uuid_list_t * p_uuid_list,
jksoft 0:8468a4403fea 174 uint8_t adv_type,
jksoft 0:8468a4403fea 175 uint8_t uuid_size,
jksoft 0:8468a4403fea 176 uint8_t * p_encoded_data,
jksoft 0:8468a4403fea 177 uint8_t * p_len)
jksoft 0:8468a4403fea 178 {
jksoft 0:8468a4403fea 179 int i;
jksoft 0:8468a4403fea 180 bool is_heading_written = false;
jksoft 0:8468a4403fea 181 uint8_t start_pos = *p_len;
jksoft 0:8468a4403fea 182
jksoft 0:8468a4403fea 183 for (i = 0; i < p_uuid_list->uuid_cnt; i++)
jksoft 0:8468a4403fea 184 {
jksoft 0:8468a4403fea 185 uint32_t err_code;
jksoft 0:8468a4403fea 186 uint8_t encoded_size;
jksoft 0:8468a4403fea 187 ble_uuid_t uuid = p_uuid_list->p_uuids[i];
jksoft 0:8468a4403fea 188
jksoft 0:8468a4403fea 189 // Find encoded uuid size.
jksoft 0:8468a4403fea 190 err_code = sd_ble_uuid_encode(&uuid, &encoded_size, NULL);
jksoft 0:8468a4403fea 191 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 192 {
jksoft 0:8468a4403fea 193 return err_code;
jksoft 0:8468a4403fea 194 }
jksoft 0:8468a4403fea 195
jksoft 0:8468a4403fea 196 // Check size.
jksoft 0:8468a4403fea 197 if (encoded_size == uuid_size)
jksoft 0:8468a4403fea 198 {
jksoft 0:8468a4403fea 199 uint8_t heading_bytes = (is_heading_written) ? 0 : 2;
jksoft 0:8468a4403fea 200
jksoft 0:8468a4403fea 201 // Check for buffer overflow
jksoft 0:8468a4403fea 202 if (*p_len + encoded_size + heading_bytes > BLE_GAP_ADV_MAX_SIZE)
jksoft 0:8468a4403fea 203 {
jksoft 0:8468a4403fea 204 return NRF_ERROR_DATA_SIZE;
jksoft 0:8468a4403fea 205 }
jksoft 0:8468a4403fea 206
jksoft 0:8468a4403fea 207 if (!is_heading_written)
jksoft 0:8468a4403fea 208 {
jksoft 0:8468a4403fea 209 // Write AD structure heading.
jksoft 0:8468a4403fea 210 (*p_len)++;
jksoft 0:8468a4403fea 211 p_encoded_data[(*p_len)++] = adv_type;
jksoft 0:8468a4403fea 212 is_heading_written = true;
jksoft 0:8468a4403fea 213 }
jksoft 0:8468a4403fea 214
jksoft 0:8468a4403fea 215 // Write UUID.
jksoft 0:8468a4403fea 216 err_code = sd_ble_uuid_encode(&uuid, &encoded_size, &p_encoded_data[*p_len]);
jksoft 0:8468a4403fea 217 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 218 {
jksoft 0:8468a4403fea 219 return err_code;
jksoft 0:8468a4403fea 220 }
jksoft 0:8468a4403fea 221 (*p_len) += encoded_size;
jksoft 0:8468a4403fea 222 }
jksoft 0:8468a4403fea 223 }
jksoft 0:8468a4403fea 224
jksoft 0:8468a4403fea 225 if (is_heading_written)
jksoft 0:8468a4403fea 226 {
jksoft 0:8468a4403fea 227 // Write length.
jksoft 0:8468a4403fea 228 p_encoded_data[start_pos] = (*p_len) - (start_pos + 1);
jksoft 0:8468a4403fea 229 }
jksoft 0:8468a4403fea 230
jksoft 0:8468a4403fea 231 return NRF_SUCCESS;
jksoft 0:8468a4403fea 232 }
jksoft 0:8468a4403fea 233
jksoft 0:8468a4403fea 234
jksoft 0:8468a4403fea 235 static uint32_t uuid_list_encode(const ble_advdata_uuid_list_t * p_uuid_list,
jksoft 0:8468a4403fea 236 uint8_t adv_type_16,
jksoft 0:8468a4403fea 237 uint8_t adv_type_128,
jksoft 0:8468a4403fea 238 uint8_t * p_encoded_data,
jksoft 0:8468a4403fea 239 uint8_t * p_len)
jksoft 0:8468a4403fea 240 {
jksoft 0:8468a4403fea 241 uint32_t err_code;
jksoft 0:8468a4403fea 242
jksoft 0:8468a4403fea 243 // Encode 16 bit UUIDs.
jksoft 0:8468a4403fea 244 err_code = uuid_list_sized_encode(p_uuid_list,
jksoft 0:8468a4403fea 245 adv_type_16,
jksoft 0:8468a4403fea 246 sizeof(uint16_le_t),
jksoft 0:8468a4403fea 247 p_encoded_data,
jksoft 0:8468a4403fea 248 p_len);
jksoft 0:8468a4403fea 249 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 250 {
jksoft 0:8468a4403fea 251 return err_code;
jksoft 0:8468a4403fea 252 }
jksoft 0:8468a4403fea 253
jksoft 0:8468a4403fea 254 // Encode 128 bit UUIDs.
jksoft 0:8468a4403fea 255 err_code = uuid_list_sized_encode(p_uuid_list,
jksoft 0:8468a4403fea 256 adv_type_128,
jksoft 0:8468a4403fea 257 sizeof(ble_uuid128_t),
jksoft 0:8468a4403fea 258 p_encoded_data,
jksoft 0:8468a4403fea 259 p_len);
jksoft 0:8468a4403fea 260 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 261 {
jksoft 0:8468a4403fea 262 return err_code;
jksoft 0:8468a4403fea 263 }
jksoft 0:8468a4403fea 264
jksoft 0:8468a4403fea 265 return NRF_SUCCESS;
jksoft 0:8468a4403fea 266 }
jksoft 0:8468a4403fea 267
jksoft 0:8468a4403fea 268
jksoft 0:8468a4403fea 269 static uint32_t conn_int_check(const ble_advdata_conn_int_t *p_conn_int)
jksoft 0:8468a4403fea 270 {
jksoft 0:8468a4403fea 271 // Check Minimum Connection Interval.
jksoft 0:8468a4403fea 272 if ((p_conn_int->min_conn_interval < 0x0006) ||
jksoft 0:8468a4403fea 273 (
jksoft 0:8468a4403fea 274 (p_conn_int->min_conn_interval > 0x0c80) &&
jksoft 0:8468a4403fea 275 (p_conn_int->min_conn_interval != 0xffff)
jksoft 0:8468a4403fea 276 )
jksoft 0:8468a4403fea 277 )
jksoft 0:8468a4403fea 278 {
jksoft 0:8468a4403fea 279 return NRF_ERROR_INVALID_PARAM;
jksoft 0:8468a4403fea 280 }
jksoft 0:8468a4403fea 281
jksoft 0:8468a4403fea 282 // Check Maximum Connection Interval.
jksoft 0:8468a4403fea 283 if ((p_conn_int->max_conn_interval < 0x0006) ||
jksoft 0:8468a4403fea 284 (
jksoft 0:8468a4403fea 285 (p_conn_int->max_conn_interval > 0x0c80) &&
jksoft 0:8468a4403fea 286 (p_conn_int->max_conn_interval != 0xffff)
jksoft 0:8468a4403fea 287 )
jksoft 0:8468a4403fea 288 )
jksoft 0:8468a4403fea 289 {
jksoft 0:8468a4403fea 290 return NRF_ERROR_INVALID_PARAM;
jksoft 0:8468a4403fea 291 }
jksoft 0:8468a4403fea 292
jksoft 0:8468a4403fea 293 // Make sure Minimum Connection Interval is not bigger than Maximum Connection Interval.
jksoft 0:8468a4403fea 294 if ((p_conn_int->min_conn_interval != 0xffff) &&
jksoft 0:8468a4403fea 295 (p_conn_int->max_conn_interval != 0xffff) &&
jksoft 0:8468a4403fea 296 (p_conn_int->min_conn_interval > p_conn_int->max_conn_interval)
jksoft 0:8468a4403fea 297 )
jksoft 0:8468a4403fea 298 {
jksoft 0:8468a4403fea 299 return NRF_ERROR_INVALID_PARAM;
jksoft 0:8468a4403fea 300 }
jksoft 0:8468a4403fea 301
jksoft 0:8468a4403fea 302 return NRF_SUCCESS;
jksoft 0:8468a4403fea 303 }
jksoft 0:8468a4403fea 304
jksoft 0:8468a4403fea 305
jksoft 0:8468a4403fea 306 static uint32_t conn_int_encode(const ble_advdata_conn_int_t * p_conn_int,
jksoft 0:8468a4403fea 307 uint8_t * p_encoded_data,
jksoft 0:8468a4403fea 308 uint8_t * p_len)
jksoft 0:8468a4403fea 309 {
jksoft 0:8468a4403fea 310 uint32_t err_code;
jksoft 0:8468a4403fea 311
jksoft 0:8468a4403fea 312 // Check for buffer overflow.
jksoft 0:8468a4403fea 313 if ((*p_len) + ADV_DATA_OFFSET + 2 * sizeof(uint16_le_t) > BLE_GAP_ADV_MAX_SIZE)
jksoft 0:8468a4403fea 314 {
jksoft 0:8468a4403fea 315 return NRF_ERROR_DATA_SIZE;
jksoft 0:8468a4403fea 316 }
jksoft 0:8468a4403fea 317
jksoft 0:8468a4403fea 318 // Check parameters.
jksoft 0:8468a4403fea 319 err_code = conn_int_check(p_conn_int);
jksoft 0:8468a4403fea 320 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 321 {
jksoft 0:8468a4403fea 322 return err_code;
jksoft 0:8468a4403fea 323 }
jksoft 0:8468a4403fea 324
jksoft 0:8468a4403fea 325 // Encode Length and AD Type.
jksoft 0:8468a4403fea 326 p_encoded_data[(*p_len)++] = 1 + 2 * sizeof(uint16_le_t);
jksoft 0:8468a4403fea 327 p_encoded_data[(*p_len)++] = BLE_GAP_AD_TYPE_SLAVE_CONNECTION_INTERVAL_RANGE;
jksoft 0:8468a4403fea 328
jksoft 0:8468a4403fea 329 // Encode Minimum and Maximum Connection Intervals.
jksoft 0:8468a4403fea 330 (*p_len) += uint16_encode(p_conn_int->min_conn_interval, &p_encoded_data[*p_len]);
jksoft 0:8468a4403fea 331 (*p_len) += uint16_encode(p_conn_int->max_conn_interval, &p_encoded_data[*p_len]);
jksoft 0:8468a4403fea 332
jksoft 0:8468a4403fea 333 return NRF_SUCCESS;
jksoft 0:8468a4403fea 334 }
jksoft 0:8468a4403fea 335
jksoft 0:8468a4403fea 336
jksoft 0:8468a4403fea 337 static uint32_t manuf_specific_data_encode(const ble_advdata_manuf_data_t * p_manuf_sp_data,
jksoft 0:8468a4403fea 338 uint8_t * p_encoded_data,
jksoft 0:8468a4403fea 339 uint8_t * p_len)
jksoft 0:8468a4403fea 340 {
jksoft 0:8468a4403fea 341 uint8_t data_size = sizeof(uint16_le_t) + p_manuf_sp_data->data.size;
jksoft 0:8468a4403fea 342
jksoft 0:8468a4403fea 343 // Check for buffer overflow.
jksoft 0:8468a4403fea 344 if ((*p_len) + ADV_DATA_OFFSET + data_size > BLE_GAP_ADV_MAX_SIZE)
jksoft 0:8468a4403fea 345 {
jksoft 0:8468a4403fea 346 return NRF_ERROR_DATA_SIZE;
jksoft 0:8468a4403fea 347 }
jksoft 0:8468a4403fea 348
jksoft 0:8468a4403fea 349 // Encode Length and AD Type.
jksoft 0:8468a4403fea 350 p_encoded_data[(*p_len)++] = 1 + data_size;
jksoft 0:8468a4403fea 351 p_encoded_data[(*p_len)++] = BLE_GAP_AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
jksoft 0:8468a4403fea 352
jksoft 0:8468a4403fea 353 // Encode Company Identifier.
jksoft 0:8468a4403fea 354 (*p_len) += uint16_encode(p_manuf_sp_data->company_identifier, &p_encoded_data[*p_len]);
jksoft 0:8468a4403fea 355
jksoft 0:8468a4403fea 356 // Encode additional manufacturer specific data.
jksoft 0:8468a4403fea 357 if (p_manuf_sp_data->data.size > 0)
jksoft 0:8468a4403fea 358 {
jksoft 0:8468a4403fea 359 if (p_manuf_sp_data->data.p_data == NULL)
jksoft 0:8468a4403fea 360 {
jksoft 0:8468a4403fea 361 return NRF_ERROR_INVALID_PARAM;
jksoft 0:8468a4403fea 362 }
jksoft 0:8468a4403fea 363 memcpy(&p_encoded_data[*p_len], p_manuf_sp_data->data.p_data, p_manuf_sp_data->data.size);
jksoft 0:8468a4403fea 364 (*p_len) += p_manuf_sp_data->data.size;
jksoft 0:8468a4403fea 365 }
jksoft 0:8468a4403fea 366
jksoft 0:8468a4403fea 367 return NRF_SUCCESS;
jksoft 0:8468a4403fea 368 }
jksoft 0:8468a4403fea 369
jksoft 0:8468a4403fea 370
jksoft 0:8468a4403fea 371 static uint32_t service_data_encode(const ble_advdata_t * p_advdata,
jksoft 0:8468a4403fea 372 uint8_t * p_encoded_data,
jksoft 0:8468a4403fea 373 uint8_t * p_len)
jksoft 0:8468a4403fea 374 {
jksoft 0:8468a4403fea 375 uint8_t i;
jksoft 0:8468a4403fea 376
jksoft 0:8468a4403fea 377 // Check parameter consistency.
jksoft 0:8468a4403fea 378 if (p_advdata->p_service_data_array == NULL)
jksoft 0:8468a4403fea 379 {
jksoft 0:8468a4403fea 380 return NRF_ERROR_INVALID_PARAM;
jksoft 0:8468a4403fea 381 }
jksoft 0:8468a4403fea 382
jksoft 0:8468a4403fea 383 for (i = 0; i < p_advdata->service_data_count; i++)
jksoft 0:8468a4403fea 384 {
jksoft 0:8468a4403fea 385 ble_advdata_service_data_t * p_service_data;
jksoft 0:8468a4403fea 386 uint8_t data_size;
jksoft 0:8468a4403fea 387
jksoft 0:8468a4403fea 388 p_service_data = &p_advdata->p_service_data_array[i];
jksoft 0:8468a4403fea 389 data_size = sizeof(uint16_le_t) + p_service_data->data.size;
jksoft 0:8468a4403fea 390
jksoft 0:8468a4403fea 391 // Encode Length and AD Type.
jksoft 0:8468a4403fea 392 p_encoded_data[(*p_len)++] = 1 + data_size;
jksoft 0:8468a4403fea 393 p_encoded_data[(*p_len)++] = BLE_GAP_AD_TYPE_SERVICE_DATA;
jksoft 0:8468a4403fea 394
jksoft 0:8468a4403fea 395 // Encode service UUID.
jksoft 0:8468a4403fea 396 (*p_len) += uint16_encode(p_service_data->service_uuid, &p_encoded_data[*p_len]);
jksoft 0:8468a4403fea 397
jksoft 0:8468a4403fea 398 // Encode additional service data.
jksoft 0:8468a4403fea 399 if (p_service_data->data.size > 0)
jksoft 0:8468a4403fea 400 {
jksoft 0:8468a4403fea 401 if (p_service_data->data.p_data == NULL)
jksoft 0:8468a4403fea 402 {
jksoft 0:8468a4403fea 403 return NRF_ERROR_INVALID_PARAM;
jksoft 0:8468a4403fea 404 }
jksoft 0:8468a4403fea 405 memcpy(&p_encoded_data[*p_len], p_service_data->data.p_data, p_service_data->data.size);
jksoft 0:8468a4403fea 406 (*p_len) += p_service_data->data.size;
jksoft 0:8468a4403fea 407 }
jksoft 0:8468a4403fea 408 }
jksoft 0:8468a4403fea 409
jksoft 0:8468a4403fea 410 return NRF_SUCCESS;
jksoft 0:8468a4403fea 411 }
jksoft 0:8468a4403fea 412
jksoft 0:8468a4403fea 413
jksoft 0:8468a4403fea 414 static uint32_t adv_data_encode(const ble_advdata_t * p_advdata,
jksoft 0:8468a4403fea 415 uint8_t * p_encoded_data,
jksoft 0:8468a4403fea 416 uint8_t * p_len)
jksoft 0:8468a4403fea 417 {
jksoft 0:8468a4403fea 418 uint32_t err_code = NRF_SUCCESS;
jksoft 0:8468a4403fea 419
jksoft 0:8468a4403fea 420 *p_len = 0;
jksoft 0:8468a4403fea 421
jksoft 0:8468a4403fea 422 // Encode name.
jksoft 0:8468a4403fea 423 if (p_advdata->name_type != BLE_ADVDATA_NO_NAME)
jksoft 0:8468a4403fea 424 {
jksoft 0:8468a4403fea 425 err_code = name_encode(p_advdata, p_encoded_data, p_len);
jksoft 0:8468a4403fea 426 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 427 {
jksoft 0:8468a4403fea 428 return err_code;
jksoft 0:8468a4403fea 429 }
jksoft 0:8468a4403fea 430 }
jksoft 0:8468a4403fea 431
jksoft 0:8468a4403fea 432 // Encode appearance.
jksoft 0:8468a4403fea 433 if (p_advdata->include_appearance)
jksoft 0:8468a4403fea 434 {
jksoft 0:8468a4403fea 435 err_code = appearance_encode(p_encoded_data, p_len);
jksoft 0:8468a4403fea 436 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 437 {
jksoft 0:8468a4403fea 438 return err_code;
jksoft 0:8468a4403fea 439 }
jksoft 0:8468a4403fea 440 }
jksoft 0:8468a4403fea 441
jksoft 0:8468a4403fea 442 // Encode flags.
jksoft 0:8468a4403fea 443 if (p_advdata->flags.size > 0)
jksoft 0:8468a4403fea 444 {
jksoft 0:8468a4403fea 445 err_code = uint8_array_encode(&p_advdata->flags,
jksoft 0:8468a4403fea 446 BLE_GAP_AD_TYPE_FLAGS,
jksoft 0:8468a4403fea 447 p_encoded_data,
jksoft 0:8468a4403fea 448 p_len);
jksoft 0:8468a4403fea 449 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 450 {
jksoft 0:8468a4403fea 451 return err_code;
jksoft 0:8468a4403fea 452 }
jksoft 0:8468a4403fea 453 }
jksoft 0:8468a4403fea 454
jksoft 0:8468a4403fea 455 // Encode TX power level.
jksoft 0:8468a4403fea 456 if (p_advdata->p_tx_power_level != NULL)
jksoft 0:8468a4403fea 457 {
jksoft 0:8468a4403fea 458 err_code = tx_power_level_encode(*p_advdata->p_tx_power_level, p_encoded_data, p_len);
jksoft 0:8468a4403fea 459 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 460 {
jksoft 0:8468a4403fea 461 return err_code;
jksoft 0:8468a4403fea 462 }
jksoft 0:8468a4403fea 463 }
jksoft 0:8468a4403fea 464
jksoft 0:8468a4403fea 465 // Encode 'more available' uuid list.
jksoft 0:8468a4403fea 466 if (p_advdata->uuids_more_available.uuid_cnt > 0)
jksoft 0:8468a4403fea 467 {
jksoft 0:8468a4403fea 468 err_code = uuid_list_encode(&p_advdata->uuids_more_available,
jksoft 0:8468a4403fea 469 BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_MORE_AVAILABLE,
jksoft 0:8468a4403fea 470 BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_MORE_AVAILABLE,
jksoft 0:8468a4403fea 471 p_encoded_data,
jksoft 0:8468a4403fea 472 p_len);
jksoft 0:8468a4403fea 473 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 474 {
jksoft 0:8468a4403fea 475 return err_code;
jksoft 0:8468a4403fea 476 }
jksoft 0:8468a4403fea 477 }
jksoft 0:8468a4403fea 478
jksoft 0:8468a4403fea 479 // Encode 'complete' uuid list.
jksoft 0:8468a4403fea 480 if (p_advdata->uuids_complete.uuid_cnt > 0)
jksoft 0:8468a4403fea 481 {
jksoft 0:8468a4403fea 482 err_code = uuid_list_encode(&p_advdata->uuids_complete,
jksoft 0:8468a4403fea 483 BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_COMPLETE,
jksoft 0:8468a4403fea 484 BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_COMPLETE,
jksoft 0:8468a4403fea 485 p_encoded_data,
jksoft 0:8468a4403fea 486 p_len);
jksoft 0:8468a4403fea 487 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 488 {
jksoft 0:8468a4403fea 489 return err_code;
jksoft 0:8468a4403fea 490 }
jksoft 0:8468a4403fea 491 }
jksoft 0:8468a4403fea 492
jksoft 0:8468a4403fea 493 // Encode 'solicited service' uuid list.
jksoft 0:8468a4403fea 494 if (p_advdata->uuids_solicited.uuid_cnt > 0)
jksoft 0:8468a4403fea 495 {
jksoft 0:8468a4403fea 496 err_code = uuid_list_encode(&p_advdata->uuids_solicited,
jksoft 0:8468a4403fea 497 BLE_GAP_AD_TYPE_SOLICITED_SERVICE_UUIDS_16BIT,
jksoft 0:8468a4403fea 498 BLE_GAP_AD_TYPE_SOLICITED_SERVICE_UUIDS_128BIT,
jksoft 0:8468a4403fea 499 p_encoded_data,
jksoft 0:8468a4403fea 500 p_len);
jksoft 0:8468a4403fea 501 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 502 {
jksoft 0:8468a4403fea 503 return err_code;
jksoft 0:8468a4403fea 504 }
jksoft 0:8468a4403fea 505 }
jksoft 0:8468a4403fea 506
jksoft 0:8468a4403fea 507 // Encode Slave Connection Interval Range.
jksoft 0:8468a4403fea 508 if (p_advdata->p_slave_conn_int != NULL)
jksoft 0:8468a4403fea 509 {
jksoft 0:8468a4403fea 510 err_code = conn_int_encode(p_advdata->p_slave_conn_int, p_encoded_data, p_len);
jksoft 0:8468a4403fea 511 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 512 {
jksoft 0:8468a4403fea 513 return err_code;
jksoft 0:8468a4403fea 514 }
jksoft 0:8468a4403fea 515 }
jksoft 0:8468a4403fea 516
jksoft 0:8468a4403fea 517 // Encode Manufacturer Specific Data.
jksoft 0:8468a4403fea 518 if (p_advdata->p_manuf_specific_data != NULL)
jksoft 0:8468a4403fea 519 {
jksoft 0:8468a4403fea 520 err_code = manuf_specific_data_encode(p_advdata->p_manuf_specific_data,
jksoft 0:8468a4403fea 521 p_encoded_data,
jksoft 0:8468a4403fea 522 p_len);
jksoft 0:8468a4403fea 523 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 524 {
jksoft 0:8468a4403fea 525 return err_code;
jksoft 0:8468a4403fea 526 }
jksoft 0:8468a4403fea 527 }
jksoft 0:8468a4403fea 528
jksoft 0:8468a4403fea 529 // Encode Service Data.
jksoft 0:8468a4403fea 530 if (p_advdata->service_data_count > 0)
jksoft 0:8468a4403fea 531 {
jksoft 0:8468a4403fea 532 err_code = service_data_encode(p_advdata, p_encoded_data, p_len);
jksoft 0:8468a4403fea 533 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 534 {
jksoft 0:8468a4403fea 535 return err_code;
jksoft 0:8468a4403fea 536 }
jksoft 0:8468a4403fea 537 }
jksoft 0:8468a4403fea 538
jksoft 0:8468a4403fea 539 return err_code;
jksoft 0:8468a4403fea 540 }
jksoft 0:8468a4403fea 541
jksoft 0:8468a4403fea 542
jksoft 0:8468a4403fea 543 static uint32_t advdata_check(const ble_advdata_t * p_advdata)
jksoft 0:8468a4403fea 544 {
jksoft 0:8468a4403fea 545 // Flags must be included in advertising data, and the BLE_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED flag must be set.
jksoft 0:8468a4403fea 546 if ((p_advdata->flags.size == 0) ||
jksoft 0:8468a4403fea 547 (p_advdata->flags.p_data == NULL) ||
jksoft 0:8468a4403fea 548 ((p_advdata->flags.p_data[0] & BLE_GAP_ADV_FLAG_BR_EDR_NOT_SUPPORTED) == 0)
jksoft 0:8468a4403fea 549 )
jksoft 0:8468a4403fea 550 {
jksoft 0:8468a4403fea 551 return NRF_ERROR_INVALID_PARAM;
jksoft 0:8468a4403fea 552 }
jksoft 0:8468a4403fea 553
jksoft 0:8468a4403fea 554 return NRF_SUCCESS;
jksoft 0:8468a4403fea 555 }
jksoft 0:8468a4403fea 556
jksoft 0:8468a4403fea 557
jksoft 0:8468a4403fea 558 static uint32_t srdata_check(const ble_advdata_t * p_srdata)
jksoft 0:8468a4403fea 559 {
jksoft 0:8468a4403fea 560 // Flags shall not be included in the scan response data.
jksoft 0:8468a4403fea 561 if (p_srdata->flags.size > 0)
jksoft 0:8468a4403fea 562 {
jksoft 0:8468a4403fea 563 return NRF_ERROR_INVALID_PARAM;
jksoft 0:8468a4403fea 564 }
jksoft 0:8468a4403fea 565
jksoft 0:8468a4403fea 566 return NRF_SUCCESS;
jksoft 0:8468a4403fea 567 }
jksoft 0:8468a4403fea 568
jksoft 0:8468a4403fea 569
jksoft 0:8468a4403fea 570 uint32_t ble_advdata_set(const ble_advdata_t * p_advdata, const ble_advdata_t * p_srdata)
jksoft 0:8468a4403fea 571 {
jksoft 0:8468a4403fea 572 uint32_t err_code;
jksoft 0:8468a4403fea 573 uint8_t len_advdata = 0;
jksoft 0:8468a4403fea 574 uint8_t len_srdata = 0;
jksoft 0:8468a4403fea 575 uint8_t encoded_advdata[BLE_GAP_ADV_MAX_SIZE];
jksoft 0:8468a4403fea 576 uint8_t encoded_srdata[BLE_GAP_ADV_MAX_SIZE];
jksoft 0:8468a4403fea 577 uint8_t * p_encoded_advdata;
jksoft 0:8468a4403fea 578 uint8_t * p_encoded_srdata;
jksoft 0:8468a4403fea 579
jksoft 0:8468a4403fea 580 // Encode advertising data (if supplied).
jksoft 0:8468a4403fea 581 if (p_advdata != NULL)
jksoft 0:8468a4403fea 582 {
jksoft 0:8468a4403fea 583 err_code = advdata_check(p_advdata);
jksoft 0:8468a4403fea 584 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 585 {
jksoft 0:8468a4403fea 586 return err_code;
jksoft 0:8468a4403fea 587 }
jksoft 0:8468a4403fea 588
jksoft 0:8468a4403fea 589 err_code = adv_data_encode(p_advdata, encoded_advdata, &len_advdata);
jksoft 0:8468a4403fea 590 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 591 {
jksoft 0:8468a4403fea 592 return err_code;
jksoft 0:8468a4403fea 593 }
jksoft 0:8468a4403fea 594 p_encoded_advdata = encoded_advdata;
jksoft 0:8468a4403fea 595 }
jksoft 0:8468a4403fea 596 else
jksoft 0:8468a4403fea 597 {
jksoft 0:8468a4403fea 598 p_encoded_advdata = NULL;
jksoft 0:8468a4403fea 599 }
jksoft 0:8468a4403fea 600
jksoft 0:8468a4403fea 601 // Encode scan response data (if supplied).
jksoft 0:8468a4403fea 602 if (p_srdata != NULL)
jksoft 0:8468a4403fea 603 {
jksoft 0:8468a4403fea 604 err_code = srdata_check(p_srdata);
jksoft 0:8468a4403fea 605 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 606 {
jksoft 0:8468a4403fea 607 return err_code;
jksoft 0:8468a4403fea 608 }
jksoft 0:8468a4403fea 609
jksoft 0:8468a4403fea 610 err_code = adv_data_encode(p_srdata, encoded_srdata, &len_srdata);
jksoft 0:8468a4403fea 611 if (err_code != NRF_SUCCESS)
jksoft 0:8468a4403fea 612 {
jksoft 0:8468a4403fea 613 return err_code;
jksoft 0:8468a4403fea 614 }
jksoft 0:8468a4403fea 615 p_encoded_srdata = encoded_srdata;
jksoft 0:8468a4403fea 616 }
jksoft 0:8468a4403fea 617 else
jksoft 0:8468a4403fea 618 {
jksoft 0:8468a4403fea 619 p_encoded_srdata = NULL;
jksoft 0:8468a4403fea 620 }
jksoft 0:8468a4403fea 621
jksoft 0:8468a4403fea 622 // Pass encoded advertising data and/or scan response data to the stack.
jksoft 0:8468a4403fea 623 return sd_ble_gap_adv_data_set(p_encoded_advdata, len_advdata, p_encoded_srdata, len_srdata);
jksoft 0:8468a4403fea 624 }