From c81b606b018d4e4555aaf970040de2347aa50d55 Mon Sep 17 00:00:00 2001 From: Glenn Ruben Bakke Date: Fri, 17 Feb 2017 21:47:38 +0100 Subject: [PATCH] nrf5/sdk: Adding support for adding 16-bit uuid's in advertisment packet. The services in paramter list can mix 16-bit and 128-bit. --- nrf5/sdk/ble_drv.c | 139 ++++++++++++++++++++++++++++++++------------- 1 file changed, 101 insertions(+), 38 deletions(-) diff --git a/nrf5/sdk/ble_drv.c b/nrf5/sdk/ble_drv.c index d91f72a6aa..a98ff15935 100644 --- a/nrf5/sdk/ble_drv.c +++ b/nrf5/sdk/ble_drv.c @@ -375,53 +375,116 @@ bool ble_drv_advertise_data(ubluepy_advertise_data_t * p_adv_params) { if (p_adv_params->num_of_services > 0) { - uint8_t size_byte_pos = byte_pos; - - // skip length byte for now, apply total length post calculation - byte_pos += BLE_ADV_LENGTH_FIELD_SIZE; - - adv_data[byte_pos] = BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_COMPLETE; - byte_pos += BLE_ADV_AD_TYPE_FIELD_SIZE; - - uint8_t uuid_total_size = 0; - uint8_t encoded_size = 0; + bool type_16bit_present = false; + bool type_128bit_present = false; for (uint8_t i = 0; i < p_adv_params->num_of_services; i++) { ubluepy_service_obj_t * p_service = (ubluepy_service_obj_t *)p_adv_params->p_services[i]; - - ble_uuid_t uuid; - uuid.type = p_service->p_uuid->uuid_vs_idx; - uuid.uuid = (uint16_t)(*(uint16_t *)&p_service->p_uuid->value[0]); - - // calculate total size of uuids - if (sd_ble_uuid_encode(&uuid, &encoded_size, NULL) != 0) { - nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError, - "Can not encode UUID, to check length.")); + if (p_service->p_uuid->type == UBLUEPY_UUID_16_BIT) { + type_16bit_present = true; } - // do encoding into the adv buffer - if (sd_ble_uuid_encode(&uuid, &encoded_size, &adv_data[byte_pos]) != 0) { - nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError, - "Can encode UUID into the advertisment packet.")); + if (p_service->p_uuid->type == UBLUEPY_UUID_128_BIT) { + type_128bit_present = true; } - - BLE_DRIVER_LOG("encoded uuid for service %u: ", 0); - for (uint8_t j = 0; j < encoded_size; j++) { - BLE_DRIVER_LOG(HEX2_FMT " ", adv_data[byte_pos + j]); - } - BLE_DRIVER_LOG("\n"); - - uuid_total_size += encoded_size; // size of entry - byte_pos += encoded_size; // relative to adv data packet - BLE_DRIVER_LOG("ADV: uuid size: %u, type: %u, uuid: %u, vs_idx: %u\n", - encoded_size, p_service->p_uuid->type, - (uint16_t)(*(uint16_t *)&p_service->p_uuid->value[0]), - p_service->p_uuid->uuid_vs_idx); } - adv_data[size_byte_pos] = (BLE_ADV_AD_TYPE_FIELD_SIZE + uuid_total_size); - } + if (type_16bit_present) { + uint8_t size_byte_pos = byte_pos; + // skip length byte for now, apply total length post calculation + byte_pos += BLE_ADV_LENGTH_FIELD_SIZE; + + adv_data[byte_pos] = BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_COMPLETE; + byte_pos += BLE_ADV_AD_TYPE_FIELD_SIZE; + + uint8_t uuid_total_size = 0; + uint8_t encoded_size = 0; + + for (uint8_t i = 0; i < p_adv_params->num_of_services; i++) { + ubluepy_service_obj_t * p_service = (ubluepy_service_obj_t *)p_adv_params->p_services[i]; + + ble_uuid_t uuid; + uuid.type = p_service->p_uuid->type; + uuid.uuid = (uint16_t)(*(uint16_t *)&p_service->p_uuid->value[0]); + + // calculate total size of uuids + if (sd_ble_uuid_encode(&uuid, &encoded_size, NULL) != 0) { + nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError, + "Can not encode UUID, to check length.")); + } + + // do encoding into the adv buffer + if (sd_ble_uuid_encode(&uuid, &encoded_size, &adv_data[byte_pos]) != 0) { + nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError, + "Can encode UUID into the advertisment packet.")); + } + + BLE_DRIVER_LOG("encoded uuid for service %u: ", 0); + for (uint8_t j = 0; j < encoded_size; j++) { + BLE_DRIVER_LOG(HEX2_FMT " ", adv_data[byte_pos + j]); + } + BLE_DRIVER_LOG("\n"); + + uuid_total_size += encoded_size; // size of entry + byte_pos += encoded_size; // relative to adv data packet + BLE_DRIVER_LOG("ADV: uuid size: %u, type: %u, uuid: %u, vs_idx: %u\n", + encoded_size, p_service->p_uuid->type, + (uint16_t)(*(uint16_t *)&p_service->p_uuid->value[0]), + p_service->p_uuid->uuid_vs_idx); + } + + adv_data[size_byte_pos] = (BLE_ADV_AD_TYPE_FIELD_SIZE + uuid_total_size); + } + + if (type_128bit_present) { + uint8_t size_byte_pos = byte_pos; + + // skip length byte for now, apply total length post calculation + byte_pos += BLE_ADV_LENGTH_FIELD_SIZE; + + adv_data[byte_pos] = BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_COMPLETE; + byte_pos += BLE_ADV_AD_TYPE_FIELD_SIZE; + + uint8_t uuid_total_size = 0; + uint8_t encoded_size = 0; + + for (uint8_t i = 0; i < p_adv_params->num_of_services; i++) { + ubluepy_service_obj_t * p_service = (ubluepy_service_obj_t *)p_adv_params->p_services[i]; + + ble_uuid_t uuid; + uuid.type = p_service->p_uuid->uuid_vs_idx; + uuid.uuid = (uint16_t)(*(uint16_t *)&p_service->p_uuid->value[0]); + + // calculate total size of uuids + if (sd_ble_uuid_encode(&uuid, &encoded_size, NULL) != 0) { + nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError, + "Can not encode UUID, to check length.")); + } + + // do encoding into the adv buffer + if (sd_ble_uuid_encode(&uuid, &encoded_size, &adv_data[byte_pos]) != 0) { + nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError, + "Can encode UUID into the advertisment packet.")); + } + + BLE_DRIVER_LOG("encoded uuid for service %u: ", 0); + for (uint8_t j = 0; j < encoded_size; j++) { + BLE_DRIVER_LOG(HEX2_FMT " ", adv_data[byte_pos + j]); + } + BLE_DRIVER_LOG("\n"); + + uuid_total_size += encoded_size; // size of entry + byte_pos += encoded_size; // relative to adv data packet + BLE_DRIVER_LOG("ADV: uuid size: %u, type: %u, uuid: %u, vs_idx: %u\n", + encoded_size, p_service->p_uuid->type, + (uint16_t)(*(uint16_t *)&p_service->p_uuid->value[0]), + p_service->p_uuid->uuid_vs_idx); + } + + adv_data[size_byte_pos] = (BLE_ADV_AD_TYPE_FIELD_SIZE + uuid_total_size); + } + } // scan response data not set if (sd_ble_gap_adv_data_set(adv_data, byte_pos, NULL, 0) != 0) {