SDP HFP record生成

接上篇文档：http://blog.csdn.net/xiaoxiaopengbo/article/details/52952925

本文主要写HFP SDP record的生成代码

闲话不多说，上硬货

HFP SDP recode main

#include <stdio.h>
#include "sdp_util.h"

unsigned char hfp_service_buffer[150];
const unsigned char   rfcomm_channel_nr = 1;
const char hfp_hf_service_name[] = "HFP SDP";


/* @param network.
 * 0 == no ability to reject a call. 
 * 1 == ability to reject a call.
 */

/* @param suported_features
 * HF bit 0: EC and/or NR function (yes/no, 1 = yes, 0 = no)
 * HF bit 1: Call waiting or three-way calling(yes/no, 1 = yes, 0 = no)
 * HF bit 2: CLI presentation capability (yes/no, 1 = yes, 0 = no)
 * HF bit 3: Voice recognition activation (yes/no, 1= yes, 0 = no)
 * HF bit 4: Remote volume control (yes/no, 1 = yes, 0 = no)
 * HF bit 5: Wide band speech (yes/no, 1 = yes, 0 = no)
 */
 /* Bit position:
 * AG bit 0: Three-way calling (yes/no, 1 = yes, 0 = no)
 * AG bit 1: EC and/or NR function (yes/no, 1 = yes, 0 = no)
 * AG bit 2: Voice recognition function (yes/no, 1 = yes, 0 = no)
 * AG bit 3: In-band ring tone capability (yes/no, 1 = yes, 0 = no)
 * AG bit 4: Attach a phone number to a voice tag (yes/no, 1 = yes, 0 = no)
 * AG bit 5: Wide band speech (yes/no, 1 = yes, 0 = no)
 */

void hfp_create_sdp_record(unsigned char * service, unsigned int service_record_handle, unsigned short service_uuid, int rfcomm_channel_nr, const char * name){
    unsigned char* attribute;
    de_create_sequence(service);

	// 0x0000 "Service Record Handle"
    de_add_number(service, DE_UINT, DE_SIZE_16, SDP_ServiceRecordHandle);
    de_add_number(service, DE_UINT, DE_SIZE_32, service_record_handle);

	// 0x0001 "Service Class ID List"
    de_add_number(service,  DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
    attribute = de_push_sequence(service);
	{
        //  "UUID for Service"
        de_add_number(attribute, DE_UUID, DE_SIZE_16, service_uuid);
        de_add_number(attribute, DE_UUID, DE_SIZE_16, SDP_GenericAudio);
    }
	de_pop_sequence(service, attribute);

	// 0x0004 "Protocol Descriptor List"
    de_add_number(service,  DE_UINT, DE_SIZE_16, SDP_ProtocolDescriptorList);
    attribute = de_push_sequence(service);
	{
		unsigned char* rfcomm = NULL;
        unsigned char* l2cpProtocol = de_push_sequence(attribute);
        {
            de_add_number(l2cpProtocol,  DE_UUID, DE_SIZE_16, SDP_L2CAPProtocol);
        }
        de_pop_sequence(attribute, l2cpProtocol);
        
        rfcomm = de_push_sequence(attribute);
        {
            de_add_number(rfcomm,  DE_UUID, DE_SIZE_16, SDP_RFCOMMProtocol);  // rfcomm_service
            de_add_number(rfcomm,  DE_UINT, DE_SIZE_8,  rfcomm_channel_nr);  // rfcomm channel
        }
        de_pop_sequence(attribute, rfcomm);
    }
    de_pop_sequence(service, attribute);

	// 0x0005 "Public Browse Group"
    de_add_number(service,  DE_UINT, DE_SIZE_16, SDP_BrowseGroupList); // public browse group
    attribute = de_push_sequence(service);
    {
        de_add_number(attribute,  DE_UUID, DE_SIZE_16, SDP_PublicBrowseGroup);
    }
    de_pop_sequence(service, attribute);

	// 0x0009 "Bluetooth Profile Descriptor List"
    de_add_number(service,  DE_UINT, DE_SIZE_16, SDP_BluetoothProfileDescriptorList);
    attribute = de_push_sequence(service);
    {
        unsigned char *sppProfile = de_push_sequence(attribute);
        {
            de_add_number(sppProfile,  DE_UUID, DE_SIZE_16, SDP_Handsfree); 
            de_add_number(sppProfile,  DE_UINT, DE_SIZE_16, 0x0107); // Verision 1.7
        }
        de_pop_sequence(attribute, sppProfile);
    }
    de_pop_sequence(service, attribute);

	// 0x0100 "Service Name"
    de_add_number(service,  DE_UINT, DE_SIZE_16, 0x0100);
    de_add_data(service,  DE_STRING, strlen(name), (unsigned char *) name);
}


void hfp_hf_create_sdp_record(unsigned char * service, unsigned int service_record_handle, int rfcomm_channel_nr, const char * name, unsigned short supported_features){
    
    hfp_create_sdp_record(service, service_record_handle, SDP_Handsfree, rfcomm_channel_nr, name);
	
    de_add_number(service, DE_UINT, DE_SIZE_16, 0x0311);    // Hands-Free Profile - SupportedFeatures
    de_add_number(service, DE_UINT, DE_SIZE_16, supported_features);
}

int main()
{
<span style="white-space:pre">	</span>int index = 0;
<span style="white-space:pre">	</span>printf("SDP TEST START\n");
<span style="white-space:pre">	</span>
<span style="white-space:pre">	</span>/* create HFP SDP record */
<span style="white-space:pre">	</span>memset(hfp_service_buffer, 0, sizeof(hfp_service_buffer));
<span style="white-space:pre">	</span>hfp_hf_create_sdp_record(hfp_service_buffer, 0x10001, rfcomm_channel_nr, hfp_hf_service_name, 0);
<span style="white-space:pre">	</span>sdp_register_service(hfp_service_buffer);


<span style="white-space:pre">	</span>for(index = 0; index < de_get_len(hfp_service_buffer); index++)
<span style="white-space:pre">		</span>printf("0x%x ",hfp_service_buffer[index]);
<span style="white-space:pre">	</span>return 0;
}

sdp_util.h

/*
 *  sdp_util.h
 */

#ifndef __SDP_UTIL_H
#define __SDP_UTIL_H

#include <stdlib.h>
#define SDP_HANDLE_ALREADY_REGISTERED                      0x80
#define SDP_QUERY_INCOMPLETE                               0x81
#define SDP_SERVICE_NOT_FOUND                              0x82
#define SDP_HANDLE_INVALID                                 0x83
#define SDP_QUERY_BUSY                                     0x84
#define BTSTACK_MEMORY_ALLOC_FAILED                        0x56

// PDU Types
typedef enum {
    SDP_Invalid = 0,
		SDP_ErrorResponse = 1,
		SDP_ServiceSearchRequest,
		SDP_ServiceSearchResponse,
		SDP_ServiceAttributeRequest,
		SDP_ServiceAttributeResponse,
		SDP_ServiceSearchAttributeRequest,
		SDP_ServiceSearchAttributeResponse
} SDP_PDU_ID_t;

// UNIVERSAL ATTRIBUTE DEFINITIONS
#define SDP_ServiceRecordHandle     0x0000
#define SDP_ServiceClassIDList      0x0001
#define SDP_ServiceRecordState      0x0002
#define SDP_ServiceID               0x0003
#define SDP_ProtocolDescriptorList  0x0004
#define SDP_BrowseGroupList         0x0005
#define SDP_LanguageBaseAttributeIDList 0x0006
#define SDP_ServiceInfoTimeToLive   0x0007
#define SDP_ServiceAvailability     0x0008
#define SDP_BluetoothProfileDescriptorList 0x0009
#define SDP_DocumentationURL        0x000a
#define SDP_ClientExecutableURL     0x000b
#define SDP_IconURL                 0x000c
#define SDP_AdditionalProtocolDescriptorList 0x000d
#define SDP_SupportedFormatsList    0x0303

// SERVICE CLASSES
#define SDP_OBEXObjectPush          0x1105
#define SDP_OBEXFileTransfer        0x1106
#define SDP_PublicBrowseGroup       0x1002
#define SDP_HSP                     0x1108
#define SDP_Headset_AG              0x1112
#define SDP_PANU                    0x1115
#define SDP_NAP                     0x1116
#define SDP_GN                      0x1117
#define SDP_Handsfree               0x111E
#define SDP_HandsfreeAudioGateway   0x111F
#define SDP_Headset_HS              0x1131
#define SDP_GenericAudio            0x1203


// PROTOCOLS
#define SDP_SDPProtocol       0x0001
#define SDP_UDPProtocol       0x0002
#define SDP_RFCOMMProtocol    0x0003
#define SDP_OBEXProtocol      0x0008
#define SDP_L2CAPProtocol     0x0100
#define SDP_BNEPProtocol      0x000F
#define SDP_AVDTPProtocol     0x0019

// OFFSETS FOR LOCALIZED ATTRIBUTES - SDP_LanguageBaseAttributeIDList
#define SDP_Offest_ServiceName      0x0000
#define SDP_Offest_ServiceDescription 0x0001
#define SDP_Offest_ProviderName     0x0002


/* API_START */

typedef enum {
    DE_NIL = 0,
		DE_UINT,
		DE_INT,
		DE_UUID,
		DE_STRING,
		DE_BOOL,
		DE_DES,
		DE_DEA,
		DE_URL
} de_type_t;

typedef enum {
    DE_SIZE_8 = 0,
		DE_SIZE_16,
		DE_SIZE_32,
		DE_SIZE_64,
		DE_SIZE_128,
		DE_SIZE_VAR_8,
		DE_SIZE_VAR_16,
		DE_SIZE_VAR_32
} de_size_t;

unsigned int big_endian_read_16( const unsigned char * buffer, int pos);
unsigned int big_endian_read_32( const unsigned char * buffer, int pos);
void big_endian_store_16(unsigned char *buffer, unsigned short pos, unsigned short value);
void big_endian_store_32(unsigned char *buffer, unsigned short pos, unsigned int value);


void	de_add_number(unsigned char *seq, de_type_t type, de_size_t size, unsigned int value);
void	de_create_sequence(unsigned char * header);
void      de_store_descriptor_with_len(unsigned char * header, de_type_t type, de_size_t size, unsigned int len);
unsigned char * de_push_sequence(unsigned char *header);
int       de_get_len(const unsigned char * header);
int       de_get_header_size(const unsigned char * header);
int       de_get_data_size(const unsigned char * header);
de_size_t de_get_size_type(const unsigned char * header);
de_type_t de_get_element_type(const unsigned char * header);
void      de_pop_sequence(unsigned char * parent, unsigned char * child);
void      de_add_data( unsigned char *seq, de_type_t type, unsigned short size, unsigned char *data);


unsigned char * sdp_get_attribute_value_for_attribute_id(unsigned char * record, unsigned short attributeID);

#endif

sdp_util.c

#include "sdp_util.h"

struct sdp_context_attribute_by_id {
    unsigned short  attributeID;
    unsigned char * attributeValue;
};

unsigned int big_endian_read_16( const unsigned char * buffer, int pos) {
    return ((unsigned short) buffer[(pos)+1]) | (((unsigned short)buffer[ pos   ]) << 8);
}

unsigned int big_endian_read_32( const unsigned char * buffer, int pos) {
    return ((unsigned int) buffer[(pos)+3]) | (((unsigned int)buffer[(pos)+2]) << 8) | (((unsigned int)buffer[(pos)+1]) << 16) | (((unsigned int) buffer[pos]) << 24);
}

void big_endian_store_16(unsigned char *buffer, unsigned short pos, unsigned short value){
    buffer[pos++] = value >> 8;
    buffer[pos++] = value;
}

void big_endian_store_32(unsigned char *buffer, unsigned short pos, unsigned int value){
    buffer[pos++] = value >> 24;
    buffer[pos++] = value >> 16;
    buffer[pos++] = value >> 8;
    buffer[pos++] = value;
}


static int sdp_traversal_attribute_by_id(unsigned short attributeID, unsigned char * attributeValue, de_type_t attributeType, de_size_t size, void *my_context){
    struct sdp_context_attribute_by_id * context = (struct sdp_context_attribute_by_id *) my_context;
    if (attributeID == context->attributeID) {
        context->attributeValue = attributeValue;
        return 1;
    }
    return 0;
}

// MARK: AttributeList traversal
typedef int (*sdp_attribute_list_traversal_callback_t)(unsigned short attributeID, unsigned char * attributeValue, de_type_t type, de_size_t size, void *context);
static void sdp_attribute_list_traverse_sequence(unsigned char * element, sdp_attribute_list_traversal_callback_t handler, void *context){
    int pos;
	int end_pos;
	unsigned short attribute_id;
	de_type_t valueType;
	de_size_t valueSize;
	unsigned char done;
	de_type_t type = de_get_element_type(element);
    if (type != DE_DES) return;
    pos = de_get_header_size(element);
    end_pos = de_get_len(element);
    while (pos < end_pos){
        de_type_t idType = de_get_element_type(element + pos);
        de_size_t idSize = de_get_size_type(element + pos);
        if (idType != DE_UINT || idSize != DE_SIZE_16) break; // wrong type
        attribute_id = big_endian_read_16(element, pos + 1);
        pos += 3;
        if (pos >= end_pos) break; // array out of bounds
        valueType = de_get_element_type(element + pos);
        valueSize = de_get_size_type(element + pos);
        done = (*handler)(attribute_id, element + pos, valueType, valueSize, context); 
        if (done) break;
        pos += de_get_len(element + pos);
    }
}


// functions to create record
static void de_store_descriptor(unsigned char * header, de_type_t type, de_size_t size){
    header[0] = (type << 3) | size; 
}

/* adds a single number value and 16+32 bit UUID to the sequence */
void de_add_number(unsigned char *seq, de_type_t type, de_size_t size, unsigned int value){
    int data_size   = big_endian_read_16(seq,1);
    int element_size = 1;   // e.g. for DE_TYPE_NIL
    de_store_descriptor(seq+3+data_size, type, size); 
    switch (size){
	case DE_SIZE_8:
		if (type != DE_NIL){
			seq[4+data_size] = value;
			element_size = 2;
		}
		break;
	case DE_SIZE_16:
		big_endian_store_16(seq, 4+data_size, value);
		element_size = 3;
		break;
	case DE_SIZE_32:
		big_endian_store_32(seq, 4+data_size, value);
		element_size = 5;
		break;
	default:
		break;
    }
    big_endian_store_16(seq, 1, data_size+element_size);
}


// MARK: DataElement creation

/* starts a new sequence in empty buffer - first call */
void de_create_sequence(unsigned char *header){
    de_store_descriptor_with_len( header, DE_DES, DE_SIZE_VAR_16, 0); // DES, 2 Byte Length
};


void de_store_descriptor_with_len(unsigned char * header, de_type_t type, de_size_t size, unsigned int len){
    header[0] = (type << 3) | size; 
    switch (size){
	case DE_SIZE_VAR_8:
		header[1] = len;
		break;
	case DE_SIZE_VAR_16:
		big_endian_store_16(header, 1, len);
		break;
	case DE_SIZE_VAR_32:
		big_endian_store_32(header, 1, len);
		break;
	default:
		break;
    }
}


/* starts a sub-sequence, @returns handle for sub-sequence */
unsigned char * de_push_sequence(unsigned char *header){
    int element_len = de_get_len(header);
    de_store_descriptor_with_len(header+element_len, DE_DES, DE_SIZE_VAR_16, 0); // DES, 2 Byte Length
    return header + element_len;
}


int de_get_len(const unsigned char *header){
    return de_get_header_size(header) + de_get_data_size(header); 
}


int de_get_header_size(const unsigned char * header){
    de_size_t de_size = de_get_size_type(header);
    if (de_size <= DE_SIZE_128) {
        return 1;
    }
    return 1 + (1 << (de_size-DE_SIZE_VAR_8));
}


int de_get_data_size(const unsigned char * header){
    unsigned int result = 0;
    de_type_t de_type = de_get_element_type(header);
    de_size_t de_size = de_get_size_type(header);
    switch (de_size){
	case DE_SIZE_VAR_8:
		result = header[1];
		break;
	case DE_SIZE_VAR_16:
		result = big_endian_read_16(header,1);
		break;
	case DE_SIZE_VAR_32:
		result = big_endian_read_32(header,1);
		break;
	default:
        // case DE_SIZE_8:
        // case DE_SIZE_16:
        // case DE_SIZE_32:
        // case DE_SIZE_64:
        // case DE_SIZE_128:
		if (de_type == DE_NIL) return 0;
		return 1 << de_size;
    }
    return result;    
}

// MARK: DataElement getter
de_size_t de_get_size_type(const unsigned char *header){
    return (de_size_t) (header[0] & 7);
}

de_type_t de_get_element_type(const unsigned char *header){
    return (de_type_t) (header[0] >> 3);
}


/* closes the current sequence and updates the parent sequence */
void de_pop_sequence(unsigned char * parent, unsigned char * child){
    int child_len = de_get_len(child);
    int data_size_parent = big_endian_read_16(parent,1);
    big_endian_store_16(parent, 1, data_size_parent + child_len);
}


/* add a single block of data, e.g. as DE_STRING, DE_URL */
void de_add_data( unsigned char *seq, de_type_t type, unsigned short size, unsigned char *data){
    int data_size   = big_endian_read_16(seq,1);
    if (size > 0xff) {
        // use 16-bit lengh information (3 byte header)
        de_store_descriptor_with_len(seq+3+data_size, type, DE_SIZE_VAR_16, size); 
        data_size += 3;
    } else {
        // use 8-bit lengh information (2 byte header)
        de_store_descriptor_with_len(seq+3+data_size, type, DE_SIZE_VAR_8, size); 
        data_size += 2;
    }
    memcpy( seq + 3 + data_size, data, size);
    data_size += size;
    big_endian_store_16(seq, 1, data_size);
}


unsigned char * sdp_get_attribute_value_for_attribute_id(unsigned char * record, unsigned short attributeID){
    struct sdp_context_attribute_by_id context;
    context.attributeValue = NULL;
    context.attributeID = attributeID;
    sdp_attribute_list_traverse_sequence(record, sdp_traversal_attribute_by_id, &context);
    return context.attributeValue;
}

sdp_server.h

#ifndef __SDP_H
#define __SDP_H
#include "btstack_linked_list.h"
typedef struct {
    // linked list - assert: first field
    btstack_linked_item_t   item;
	
    unsigned int        service_record_handle;
    unsigned char *       service_record;
} service_record_item_t;

/**
 * @brief Register Service Record with database using ServiceRecordHandle stored in record
 * @pre AttributeIDs are in ascending order
 * @pre ServiceRecordHandle is first attribute and valid
 * @param record is not copied!
 * @result status
 */
unsigned char sdp_register_service(const unsigned char * record);

/**
 * @brief gets service record handle from record
 * @resutl service record handle or 0
 */
unsigned int sdp_get_service_record_handle(const unsigned char * record);



#endif

sdp_server.c

#include "sdp_server.h"
#include "sdp_util.h"



// max reserved ServiceRecordHandle
#define maxReservedServiceRecordHandle 0xffff

// registered service records
static btstack_linked_list_t sdp_service_records = NULL;

service_record_item_t * btstack_memory_service_record_item_get(void){
    return (service_record_item_t*) malloc(sizeof(service_record_item_t));
}

static service_record_item_t * sdp_get_record_item_for_handle(unsigned int handle){
    btstack_linked_item_t *it;
    for (it = (btstack_linked_item_t *) sdp_service_records; it ; it = it->next){
        service_record_item_t * item = (service_record_item_t *) it;
        if (item->service_record_handle == handle){
            return item;
        }
    }
    return NULL;
}

/**
 * @brief Register Service Record with database using ServiceRecordHandle stored in record
 * @pre AttributeIDs are in ascending order
 * @pre ServiceRecordHandle is first attribute and valid
 * @param record is not copied!
 * @result status
 */
unsigned char sdp_register_service(const unsigned char * record){
	service_record_item_t * newRecordItem;
	// validate service record handle. it must: exist, be in valid range, not have been already used
    unsigned int record_handle = sdp_get_service_record_handle(record);
	if (!record_handle) return SDP_HANDLE_INVALID;
	if (record_handle <= maxReservedServiceRecordHandle) return SDP_HANDLE_INVALID;
	if (sdp_get_record_item_for_handle(record_handle)) return SDP_HANDLE_ALREADY_REGISTERED;

	// alloc memory for new service_record_item
    newRecordItem = btstack_memory_service_record_item_get();
    if (!newRecordItem) return BTSTACK_MEMORY_ALLOC_FAILED;

	// set handle and record
    newRecordItem->service_record_handle = record_handle;
    newRecordItem->service_record = (unsigned char*) record;

	printf("record_handle = 0x%x\n",record_handle);
    
    // add to linked list
    btstack_linked_list_add(&sdp_service_records, (btstack_linked_item_t *) newRecordItem);
    
    return 0;

}


/**
 * @brief gets service record handle from record
 * @resutl service record handle or 0
 */
unsigned int sdp_get_service_record_handle(const unsigned char * record)
{
	unsigned char * serviceRecordHandleAttribute = sdp_get_attribute_value_for_attribute_id((unsigned char *)record, SDP_ServiceRecordHandle);
	if (!serviceRecordHandleAttribute) return 0;
	if (de_get_element_type(serviceRecordHandleAttribute) != DE_UINT) return 0;
	if (de_get_size_type(serviceRecordHandleAttribute) != DE_SIZE_32) return 0;
    return big_endian_read_32(serviceRecordHandleAttribute, 1); 
}

链表头文件

/*
 *  btstack_linked_list.h
 */

#ifndef __BTSTACK_LINKED_LIST_H
#define __BTSTACK_LINKED_LIST_H

#if defined __cplusplus
extern "C" {
#endif
	
typedef struct btstack_linked_item {
    struct btstack_linked_item *next; // <-- next element in list, or NULL
} btstack_linked_item_t;

typedef btstack_linked_item_t * btstack_linked_list_t;

typedef struct {
	int advance_on_next;
    btstack_linked_item_t * prev;	// points to the item before the current one
    btstack_linked_item_t * curr;	// points to the current item (to detect item removal)
} btstack_linked_list_iterator_t;


// test if list is empty
int                     btstack_linked_list_empty(btstack_linked_list_t * list);
// add item to list as first element
void                    btstack_linked_list_add(btstack_linked_list_t * list, btstack_linked_item_t *item);       
// add item to list as last element
void                    btstack_linked_list_add_tail(btstack_linked_list_t * list, btstack_linked_item_t *item); 
// remove item from list
int                     btstack_linked_list_remove(btstack_linked_list_t * list, btstack_linked_item_t *item); 
// find the last item in the list
btstack_linked_item_t * btstack_linked_list_get_last_item(btstack_linked_list_t * list);   

/**
 * @brief Counts number of items in list
 * @returns number of items in list
 */
int btstack_linked_list_count(btstack_linked_list_t * list);

//
// iterator for linked lists. allows to remove current element.
// robust against removal of current element by btstack_linked_list_remove.
//
void            btstack_linked_list_iterator_init(btstack_linked_list_iterator_t * it, btstack_linked_list_t * list);
int             btstack_linked_list_iterator_has_next(btstack_linked_list_iterator_t * it);
btstack_linked_item_t * btstack_linked_list_iterator_next(btstack_linked_list_iterator_t * it);
void            btstack_linked_list_iterator_remove(btstack_linked_list_iterator_t * it);

void test_linked_list(void);

#if defined __cplusplus
}
#endif

#endif // __BTSTACK_LINKED_LIST_H

链表实现文件

#include "btstack_linked_list.h"
#include <stdlib.h>
#include <stdio.h>

/**
 * tests if list is empty
 */
int  btstack_linked_list_empty(btstack_linked_list_t * list){
    return *list == (void *) 0;
}

/**
 * btstack_linked_list_get_last_item
 */
btstack_linked_item_t * btstack_linked_list_get_last_item(btstack_linked_list_t * list){        // <-- find the last item in the list
    btstack_linked_item_t *lastItem = NULL;
    btstack_linked_item_t *it;
    for (it = *list; it ; it = it->next){
        if (it) {
            lastItem = it;
        }
    }
    return lastItem;
}


/**
 * btstack_linked_list_add
 */
void btstack_linked_list_add(btstack_linked_list_t * list, btstack_linked_item_t *item){        // <-- add item to list
    // check if already in list
    btstack_linked_item_t *it;
    for (it = *list; it ; it = it->next){
        if (it == item) {
            return;
        }
    }
    // add first
    item->next = *list;
    *list = item;
}

void btstack_linked_list_add_tail(btstack_linked_list_t * list, btstack_linked_item_t *item){   // <-- add item to list as last element
    // check if already in list
    btstack_linked_item_t *it;
    for (it = (btstack_linked_item_t *) list; it->next ; it = it->next){
        if (it->next == item) {
            return;
        }
    }
    item->next = (btstack_linked_item_t*) 0;
    it->next = item;
}

/**
 * Remove data_source from run loop
 *
 * @note: assumes that btstack_data_source_t.next is first element in data_source
 */
int  btstack_linked_list_remove(btstack_linked_list_t * list, btstack_linked_item_t *item){    // <-- remove item from list
	btstack_linked_item_t *it;
	if (!item) return -1;
    
    for (it = (btstack_linked_item_t *) list; it ; it = it->next){
        if (it->next == item){
            it->next =  item->next;
            return 0;
        }
    }
    return -1;
}

/**
 * @returns number of items in list
 */
 int btstack_linked_list_count(btstack_linked_list_t * list){
    btstack_linked_item_t *it;
    int counter = 0;
    for (it = (btstack_linked_item_t *) list; it->next ; it = it->next) {
        counter++;
    }
    return counter; 
}

//
// Linked List Iterator implementation
//

void btstack_linked_list_iterator_init(btstack_linked_list_iterator_t * it, btstack_linked_list_t * head){
    it->advance_on_next = 0;
    it->prev = (btstack_linked_item_t*) head;
    it->curr = * head;
}

int btstack_linked_list_iterator_has_next(btstack_linked_list_iterator_t * it){
    // log_info("btstack_linked_list_iterator_has_next: advance on next %u, it->prev %p, it->curr %p", it->advance_on_next, it->prev, it->curr);
    if (!it->advance_on_next){
        return it->curr != NULL;
    }
    if (it->prev->next != it->curr){
        // current item has been removed
        return it->prev->next != NULL;
    }
    // current items has not been removed
    return it->curr->next != NULL;
}

btstack_linked_item_t * btstack_linked_list_iterator_next(btstack_linked_list_iterator_t * it){
    if (it->advance_on_next){
        if (it->prev->next == it->curr){
            it->prev = it->curr;
            it->curr = it->curr->next;
        } else {
            // curr was removed from the list, set it again but don't advance prev
            it->curr = it->prev->next;
        }
    } else {
        it->advance_on_next = 1;
    }
    return it->curr;
}

void btstack_linked_list_iterator_remove(btstack_linked_list_iterator_t * it){
    it->curr = it->curr->next;
    it->prev->next = it->curr;
    it->advance_on_next = 0;
}

效果图:

后续会带来sdp server和client的PDU交互code