sdp.c

linux下的蓝牙协议栈

	void **seqDTDs, **seqs;
	int i, seqlen;
	sdp_list_t *p;

	seqlen = sdp_list_len(proto);
	seqDTDs = (void **)malloc(seqlen * sizeof(void *));
	seqs = (void **)malloc(seqlen * sizeof(void *));
	for (i = 0, p = proto; p; p = p->next, i++) {
		sdp_list_t *elt = (sdp_list_t *)p->data;
		sdp_data_t *s;
		int pslen = 0;
		for (; elt && pslen < sizeof(dtds); elt = elt->next, pslen++) {
			sdp_data_t *d = (sdp_data_t *)elt->data;
			dtds[pslen] = &d->dtd;
			switch (d->dtd) {
			case SDP_UUID16:
				values[pslen] = &((uuid_t *)d)->value.uuid16;
				break;
			case SDP_UUID32:
				values[pslen] = &((uuid_t *)d)->value.uuid32;
				break;
			case SDP_UUID128:
				values[pslen] = &((uuid_t *)d)->value.uuid128;
				break;
			case SDP_UINT8:
				values[pslen] = &d->val.uint8;
				break;
			case SDP_UINT16:
				values[pslen] = &d->val.uint16;
				break;
			case SDP_SEQ8:
			case SDP_SEQ16:
			case SDP_SEQ32:
				values[pslen] = d;
				break;
			// FIXME: more
			}
		}
		s = sdp_seq_alloc(dtds, values, pslen);
		if (s) {
			seqDTDs[i] = &s->dtd;
			seqs[i] = s;
		}
	}
	seq = sdp_seq_alloc(seqDTDs, seqs, seqlen);
	free(seqDTDs);
	free(seqs);
	return seq;
}

/*
 * sets the access protocols of the service specified
 * to the value specified in "access_proto"
 *
 * Note that if there are alternate mechanisms by 
 * which the service is accessed, then they should 
 * be specified as sequences 
 *
 * Using a value of NULL for accessProtocols has
 * effect of removing this attribute (if previously set)
 * 
 * This function replaces the existing sdp_access_proto_t
 * structure (if any) with the new one specified.
 *
 * returns 0 if successful or -1 if there is a failure.
 */
int sdp_set_access_protos(sdp_record_t *rec, const sdp_list_t *ap)
{
	const sdp_list_t *p;
	sdp_data_t *protos = 0;

	for (p = ap; p; p = p->next) {
		sdp_data_t *seq = access_proto_to_dataseq(rec, (sdp_list_t *)p->data);
		protos = sdp_seq_append(protos, seq);
	}
	sdp_attr_add(rec, SDP_ATTR_PROTO_DESC_LIST, protos);
	return 0;
}

/*
 * set the "LanguageBase" attributes of the service record
 * record to the value specified in "langAttrList".
 *
 * "langAttrList" is a linked list of "sdp_lang_attr_t"
 * objects, one for each language in which user visible
 * attributes are present in the service record.
 *
 * Using a value of NULL for langAttrList has
 * effect of removing this attribute (if previously set)
 * 
 * This function replaces the exisiting sdp_lang_attr_t
 * structure (if any) with the new one specified.
 *
 * returns 0 if successful or -1 if there is a failure.
 */
int sdp_set_lang_attr(sdp_record_t *rec, const sdp_list_t *seq)
{
	uint8_t uint16 = SDP_UINT16;
	int status = 0, i = 0, seqlen = sdp_list_len(seq);
	void **dtds = (void **)malloc(3 * seqlen * sizeof(void *));
	void **values = (void **)malloc(3 * seqlen * sizeof(void *));
	const sdp_list_t *p;

	for (p = seq; p; p = p->next) {
		sdp_lang_attr_t *lang = (sdp_lang_attr_t *)p->data;
		if (!lang) {
			status = -1;
			break;
		}
		dtds[i] = &uint16;
		values[i] = &lang->code_ISO639;
		i++;
		dtds[i] = &uint16;
		values[i] = &lang->encoding;
		i++;
		dtds[i] = &uint16;
		values[i] = &lang->base_offset;
		i++;
	}
	if (status == 0) {
		sdp_data_t *seq = sdp_seq_alloc(dtds, values, 3 * seqlen);
		sdp_attr_add(rec, SDP_ATTR_LANG_BASE_ATTR_ID_LIST, seq);
	}
	free(dtds);
	free(values);
	return status;
}

/*
 * set the "ServiceID" attribute of the service. 
 * 
 * This is the UUID of the service. 
 * 
 * returns 0 if successful or -1 if there is a failure.
 */
void sdp_set_service_id(sdp_record_t *rec, uuid_t uuid)
{
	switch (uuid.type) {
	case SDP_UUID16:
		sdp_attr_add_new(rec, SDP_ATTR_SERVICE_ID, SDP_UUID16, &uuid.value.uuid16);
		break;
	case SDP_UUID32:
		sdp_attr_add_new(rec, SDP_ATTR_SERVICE_ID, SDP_UUID32, &uuid.value.uuid32);
		break;
	case SDP_UUID128:
		sdp_attr_add_new(rec, SDP_ATTR_SERVICE_ID, SDP_UUID128, &uuid.value.uuid128);
		break;
	}
	sdp_pattern_add_uuid(rec, &uuid);
}

/*
 * set the GroupID attribute of the service record defining a group. 
 * 
 * This is the UUID of the group. 
 * 
 * returns 0 if successful or -1 if there is a failure.
 */
void sdp_set_group_id(sdp_record_t *rec, uuid_t uuid)
{
	switch (uuid.type) {
	case SDP_UUID16:
		sdp_attr_add_new(rec, SDP_ATTR_GROUP_ID, SDP_UUID16, &uuid.value.uuid16);
		break;
	case SDP_UUID32:
		sdp_attr_add_new(rec, SDP_ATTR_GROUP_ID, SDP_UUID32, &uuid.value.uuid32);
		break;
	case SDP_UUID128:
		sdp_attr_add_new(rec, SDP_ATTR_GROUP_ID, SDP_UUID128, &uuid.value.uuid128);
		break;
	}
	sdp_pattern_add_uuid(rec, &uuid);
}

/*
 * set the ProfileDescriptorList attribute of the service record
 * pointed to by record to the value specified in "profileDesc".
 *
 * Each element in the list is an object of type
 * sdp_profile_desc_t which is a definition of the
 * Bluetooth profile that this service conforms to.
 *
 * Using a value of NULL for profileDesc has
 * effect of removing this attribute (if previously set)
 * 
 * This function replaces the exisiting ProfileDescriptorList
 * structure (if any) with the new one specified.
 *
 * returns 0 if successful or -1 if there is a failure.
 */
int sdp_set_profile_descs(sdp_record_t *rec, const sdp_list_t *profiles)
{
	int status = 0;
	uint8_t uuid16 = SDP_UUID16;
	uint8_t uuid32 = SDP_UUID32;
	uint8_t uuid128 = SDP_UUID128;
	uint8_t uint16 = SDP_UINT16;
	int i = 0, seqlen = sdp_list_len(profiles);
	void **seqDTDs = (void **)malloc(seqlen * sizeof(void *));
	void **seqs = (void **)malloc(seqlen * sizeof(void *));
	const sdp_list_t *p;

	for (p = profiles; p; p = p->next) {
		sdp_data_t *seq;
		void *dtds[2], *values[2];
		sdp_profile_desc_t *profile = (sdp_profile_desc_t *)p->data;
		if (!profile) {
			status = -1;
			break;
		}
		switch (profile->uuid.type) {
		case SDP_UUID16:
			dtds[0] = &uuid16;
			values[0] = &profile->uuid.value.uuid16;
			break;
		case SDP_UUID32:
			dtds[0] = &uuid32;
			values[0] = &profile->uuid.value.uuid32;
			break;
		case SDP_UUID128:
			dtds[0] = &uuid128;
			values[0] = &profile->uuid.value.uuid128;
			break;
		default:
			status = -1;
			break;
		}
		dtds[1] = &uint16;
		values[1] = &profile->version;
		seq = sdp_seq_alloc(dtds, values, 2);
		if (seq) {
			seqDTDs[i] = &seq->dtd;
			seqs[i] = seq;
			sdp_pattern_add_uuid(rec, &profile->uuid);
		}
		i++;
	}
	if (status == 0) {
		sdp_data_t *pAPSeq = sdp_seq_alloc(seqDTDs, seqs, seqlen);
		sdp_attr_add(rec, SDP_ATTR_PFILE_DESC_LIST, pAPSeq);
	}
	free(seqDTDs);
	free(seqs);
	return status;
}

/*
 * sets various URL attributes of the service
 * pointed to by record. The URL include
 *
 * client: a URL to the client's
 *   platform specific (WinCE, PalmOS) executable
 *   code that can be used to access this service.
 *
 * doc: a URL pointing to service documentation
 *
 * icon: a URL to an icon that can be used to represent
 *   this service.
 *
 * Note that you need to pass NULL for any URLs
 * that you don't want to set or remove
 */
void sdp_set_url_attr(sdp_record_t *rec, const char *client, const char *doc, const char *icon)
{
	sdp_attr_add_new(rec, SDP_ATTR_CLNT_EXEC_URL, SDP_URL_STR8, client);
	sdp_attr_add_new(rec, SDP_ATTR_DOC_URL, SDP_URL_STR8, doc);
	sdp_attr_add_new(rec, SDP_ATTR_ICON_URL, SDP_URL_STR8, icon);
}

/*
 * The code in this function is executed only once per
 * thread. We compute the actual bit value of the Bluetooth
 * base UUID which is a string defined in bt_std_values.h 
 * and is assumed to be of the standard form with "-" separators.
 *
 * The algorithm however converts the string to 4 unsigned longs
 * using the strtoul() and assigns the values in sequence to
 * the 128bit value
 */
uint128_t *sdp_create_base_uuid(void)
{
	char baseStr[128];
	int delim = '-';
	unsigned long dataLongValue;
	char *delimPtr;
	char *dataPtr;
	char temp[10];
	int toBeCopied;
	char *data;

	if (bluetooth_base_uuid == NULL) {
		strcpy(baseStr, BASE_UUID);
		bluetooth_base_uuid = (uint128_t *)malloc(sizeof(uint128_t));
		data = bluetooth_base_uuid->data;
		memset(data, '\0', sizeof(uint128_t));
		memset(temp, '\0', 10);
		dataPtr = baseStr;
		delimPtr = NULL;
		delimPtr = strchr(dataPtr, delim);
		toBeCopied = delimPtr - dataPtr;
		if (toBeCopied != 8) {
			SDPDBG("To be copied(1) : %d\n", toBeCopied);
			return NULL;
		}
		strncpy(temp, dataPtr, toBeCopied);
		dataLongValue = htonl(strtoul(temp, NULL, 16));
		memcpy(&data[0], &dataLongValue, 4);

		/*
		 * Get the next 4 bytes (note that there is a "-"
		 * between them now)
		 */
		memset(temp, '\0', 10);
		dataPtr = delimPtr + 1;
		delimPtr = strchr(dataPtr, delim);
		toBeCopied = delimPtr - dataPtr;
		if (toBeCopied != 4) {
			SDPDBG("To be copied(2) : %d\n", toBeCopied);
			return NULL;
		}
		strncpy(temp, dataPtr, toBeCopied);
		dataPtr = delimPtr + 1;
		delimPtr = strchr(dataPtr, delim);
		toBeCopied = delimPtr - dataPtr;
		if (toBeCopied != 4) {
			SDPDBG("To be copied(3) : %d\n", toBeCopied);
			return NULL;
		}
		strncat(temp, dataPtr, toBeCopied);
		dataLongValue = htonl(strtoul(temp, NULL, 16));
		memcpy(&data[4], &dataLongValue, 4);

		/*
		 * Get the last 4 bytes (note that there are 6 bytes
		 * after the last separator, which is truncated (2+4)
		 */
		memset(temp, '\0', 10);
		dataPtr = delimPtr + 1;
		dataPtr = delimPtr + 1;
		delimPtr = strchr(dataPtr, delim);
		toBeCopied = delimPtr - dataPtr;
		if (toBeCopied != 4) {
			SDPDBG("To be copied(4) : %d\n", toBeCopied);
			return NULL;
		}
		strncpy(temp, dataPtr, toBeCopied);
		strncat(temp, (delimPtr + 1), 4);
		dataLongValue = htonl(strtoul(temp, NULL, 16));
		memcpy(&data[8], &dataLongValue, 4);
		dataLongValue = htonl(strtoul(delimPtr + 5, NULL, 16));
		memcpy(&data[12], &dataLongValue, 4);
	}
	return bluetooth_base_uuid;
}

uuid_t *sdp_uuid16_create(uuid_t *u, uint16_t val)
{
	memset(u, 0, sizeof(uuid_t));
	u->type = SDP_UUID16;
	u->value.uuid16 = val;
	return u;
}

uuid_t *sdp_uuid32_create(uuid_t *u, uint32_t val)
{
	memset(u, 0, sizeof(uuid_t));
	u->type = SDP_UUID32;
	u->value.uuid32 = val;
	return u;
}

uuid_t *sdp_uuid128_create(uuid_t *u, const void *val)
{ 
	memset(u, 0, sizeof(uuid_t));
	u->type = SDP_UUID128;
	memcpy(&u->value.uuid128, val, sizeof(uint128_t));
	return u;
}

/*
 * UUID comparison function
 * returns 0 if uuidValue1 == uuidValue2 else -1
 */
int sdp_uuid16_cmp(const void *p1, const void *p2)
{
	const uuid_t *u1 = (const uuid_t *)p1;
	const uuid_t *u2 = (const uuid_t *)p2;
	return memcmp(&u1->value.uuid16, &u2->value.uuid16, sizeof(uint16_t));
}

/*
 * UUID comparison function
 * returns 0 if uuidValue1 == uuidValue2 else -1
 */
int sdp_uuid128_cmp(const void *p1, const void *p2)
{
	const uuid_t *u1 = (const uuid_t *)p1;
	const uuid_t *u2 = (const uuid_t *)p2;
	return memcmp(&u1->value.uuid128, &u2->value.uuid128, sizeof(uint128_t));
}

/*
 * 128 to 16 bit and 32 to 16 bit UUID conversion functions
 * yet to be implemented. Note that the input is in NBO in
 * both 32 and 128 bit UUIDs and conversion is needed
 */
void sdp_uuid16_to_uuid128(uuid_t *uuid128, uuid_t *uuid16)
{
	/*
	 * We have a 16 bit value, which needs to be added to
	 * bytes 3 and 4 (at indices 2 and 3) of the Bluetooth base
	 */
	unsigned short data1;

	// allocate a 128bit UUID and init to the Bluetooth base UUID
	uint128_t *pBTBase128Bit = sdp_create_base_uuid();
	uuid128->value.uuid128 = *pBTBase128Bit;
	uuid128->type = SDP_UUID128;

	// extract bytes 2 and 3 of 128bit BT base UUID
	memcpy(&data1, &pBTBase128Bit->data[2], 2);

	// add the given UUID (16 bits)
	data1 += htons(uuid16->value.uuid16);

	// set bytes 2 and 3 of the 128 bit value
	memcpy(&uuid128->value.uuid128.data[2], &data1, 2);
}

void sdp_uuid32_to_uuid128(uuid_t *uuid128, uuid_t *uuid32)
{
	/*
	 * We have a 32 bit value, which needs to be added to
	 * bytes 1->4 (at indices 0 thru 3) of the Bluetooth base
	 */
	unsigned int data0;

	// allocate a 128bit UUID and init to the Bluetooth base UUID
	uint128_t *pBTBase128Bit = sdp_create_base_uuid();
	uuid128->value.uuid128 = *pBTBase128Bit;
	uuid128->type = SDP_UUID128;

	// extract first 4 bytes
	memcpy(&data0, &pBTBase128Bit->data[0], 4);

	// add the given UUID (32bits)
	data0 += htonl(uuid32->value.uuid32);

	// set the 4 bytes of the 128 bit value
	memcpy(&uuid128->value.uuid128.data[0], &data0, 4);
}

uuid_t *sdp_uuid_to_uuid128(uuid_t *uuid)
{
	uuid_t *uuid128 = (uuid_t *)malloc(sizeof(uuid_t));
	memset(uuid128, 0, sizeof(uuid_t));
	switch (uuid->type) {
	case SDP_UUID128:
		*uuid128 = *uuid;
		break;
	case SDP_UUID32:
		sdp_uuid32_to_uuid128(uuid128, uuid);
		break;
	case SDP_UUID16:
		sdp_uuid16_to_uuid128(uuid128, uuid);
		break;
	}
	return uuid128;
}

/* 
 * converts a 128-bit uuid to a 16/32-bit one if possible
 * returns true if uuid contains a 16/32-bit UUID at exit
 */
int sdp_uuid128_to_uuid(uuid_t *uuid)
{
	extern uint128_t *sdp_create_base_uuid();
	int i;
	uint128_t *b = sdp_create_base_uuid();
	uint128_t *u = &uuid->value.uuid128;
	uint32_t data;

	if (uuid->type != SDP_UUID128)
		return 1;

	for (i = 4; i < sizeof(b->data); i++)
		if (b->data[i] != u->data[i])
			return 0;

	memcpy(&data, u->data, 4);
	data = htonl(data);
	if (data <= 0xffff) {
		uuid->type = SDP_UUID16;
		uuid->value.uuid16 = (uint16_t)data;
	} else {
		uuid->type = SDP_UUID32;
		uuid->value.uuid32 = data;
	}
	return 1;
}

/*
 * convert a UUID to the 16-bit short-form
 */
int sdp_uuid_to_proto(uuid_t *uuid)
{
	uuid_t u = *uuid;
	if (sdp_uuid128_to_uuid(&u)) {
		switch (u.type) {
		case SDP_UUID16:
			return u.value.uuid16;
		case SDP_UUID32:
			return u.value.uuid32;
		}
	}
	return 0;
}

int sdp_uuid_extract(const char *p, uuid_t *uuid, int *scanned)
{
	uint8_t type = *(const uint8_t *)p;

	if (!SDP_IS_UUID(type)) {
		SDPERR("Unknown data type : %d expecting a svc UUID\n", type);
		return -1;
	}
	p += sizeof(uint8_t);
	*scanned += sizeof(uint8_t);
	if (type == SDP_UUID16) {
		sdp_uuid16_create(uuid, ntohs(sdp_get_unaligned((uint16_t *)p)));