sdp.c

实现蓝牙的各种profile

	return sdp_data_alloc_with_length(SDP_SEQ8, seq, length[i]);
}

sdp_data_t *sdp_seq_alloc(void **dtds, void **values, int len)
{
	sdp_data_t *curr = NULL, *seq = NULL;
	int i;

	for (i = 0; i < len; i++) {
		sdp_data_t *data;
		uint8_t dtd = *(uint8_t *) dtds[i];

		if (dtd >= SDP_SEQ8 && dtd <= SDP_ALT32)
			data = (sdp_data_t *) values[i];
		else
			data = sdp_data_alloc(dtd, values[i]);

		if (!data)
			return NULL;

		if (curr)
			curr->next = data;
		else
			seq = data;

		curr = data;
	}

	return sdp_data_alloc(SDP_SEQ8, seq);
}

static void extract_svclass_uuid(sdp_data_t *data, uuid_t *uuid)
{
	sdp_data_t *d;

	if (!data || data->dtd < SDP_SEQ8 || data->dtd > SDP_SEQ32)
		return;

	d = data->val.dataseq;
	if (!d)
		return;

	if (d->dtd < SDP_UUID16 || d->dtd > SDP_UUID128)
		return;

	*uuid = d->val.uuid;
}

int sdp_attr_add(sdp_record_t *rec, uint16_t attr, sdp_data_t *d)
{
	sdp_data_t *p = sdp_data_get(rec, attr);

	if (p)
		return -1;

	d->attrId = attr;
	rec->attrlist = sdp_list_insert_sorted(rec->attrlist, d, sdp_attrid_comp_func);

	if (attr == SDP_ATTR_SVCLASS_ID_LIST)
		extract_svclass_uuid(d, &rec->svclass);

	return 0;
}

void sdp_attr_remove(sdp_record_t *rec, uint16_t attr)
{
	sdp_data_t *d = sdp_data_get(rec, attr);

	if (d)
		rec->attrlist = sdp_list_remove(rec->attrlist, d);

	if (attr == SDP_ATTR_SVCLASS_ID_LIST)
		memset(&rec->svclass, 0, sizeof(rec->svclass));
}

void sdp_set_seq_len(uint8_t *ptr, uint32_t length)
{
	uint8_t dtd = *(uint8_t *) ptr++;

	switch (dtd) {
	case SDP_SEQ8:
	case SDP_ALT8:
	case SDP_TEXT_STR8:
	case SDP_URL_STR8:
		*(uint8_t *)ptr = (uint8_t) length;
		break;
	case SDP_SEQ16:
	case SDP_ALT16:
	case SDP_TEXT_STR16:
	case SDP_URL_STR16:
		bt_put_unaligned(htons(length), (uint16_t *) ptr);
		break;
	case SDP_SEQ32:
	case SDP_ALT32:
	case SDP_TEXT_STR32:
	case SDP_URL_STR32:
		bt_put_unaligned(htonl(length), (uint32_t *) ptr);
		break;
	}
}

int sdp_set_data_type(sdp_buf_t *buf, uint8_t dtd)
{
	int orig = buf->data_size;
	uint8_t *p = buf->data + buf->data_size;

	*p++ = dtd;
	buf->data_size += sizeof(uint8_t);

	switch (dtd) {
	case SDP_SEQ8:
	case SDP_TEXT_STR8:
	case SDP_URL_STR8:
	case SDP_ALT8:
		buf->data_size += sizeof(uint8_t);
		break;
	case SDP_SEQ16:
	case SDP_TEXT_STR16:
	case SDP_URL_STR16:
	case SDP_ALT16:
		buf->data_size += sizeof(uint16_t);
		break;
	case SDP_SEQ32:
	case SDP_TEXT_STR32:
	case SDP_URL_STR32:
	case SDP_ALT32:
		buf->data_size += sizeof(uint32_t);
		break;
	}

	return buf->data_size - orig;
}

void sdp_set_attrid(sdp_buf_t *buf, uint16_t attr)
{
	uint8_t *p = buf->data;

	// data type for attr
	*p++ = SDP_UINT16;
	buf->data_size = sizeof(uint8_t);
	bt_put_unaligned(htons(attr), (uint16_t *) p);
	p += sizeof(uint16_t);
	buf->data_size += sizeof(uint16_t);
}

static int get_data_size(sdp_buf_t *buf, sdp_data_t *sdpdata)
{
	sdp_data_t *d;
	int n = 0;

	for (d = sdpdata->val.dataseq; d; d = d->next)
		n += sdp_gen_pdu(buf, d);

	return n;
}

int sdp_gen_pdu(sdp_buf_t *buf, sdp_data_t *d)
{
	uint32_t pdu_size = 0, data_size = 0;
	unsigned char *src = NULL, is_seq = 0, is_alt = 0;
	uint8_t dtd = d->dtd;
	uint16_t u16;
	uint32_t u32;
	uint64_t u64;
	uint128_t u128;
	uint8_t *seqp = buf->data + buf->data_size;

	pdu_size = sdp_set_data_type(buf, dtd);

	switch (dtd) {
	case SDP_DATA_NIL:
		break;
	case SDP_UINT8:
		src = &d->val.uint8;
		data_size = sizeof(uint8_t);
		break;
	case SDP_UINT16:
		u16 = htons(d->val.uint16);
		src = (unsigned char *) &u16;
		data_size = sizeof(uint16_t);
		break;
	case SDP_UINT32:
		u32 = htonl(d->val.uint32);
		src = (unsigned char *) &u32;
		data_size = sizeof(uint32_t);
		break;
	case SDP_UINT64:
		u64 = hton64(d->val.uint64);
		src = (unsigned char *) &u64;
		data_size = sizeof(uint64_t);
		break;
	case SDP_UINT128:
		hton128(&d->val.uint128, &u128);
		src = (unsigned char *) &u128;
		data_size = sizeof(uint128_t);
		break;
	case SDP_INT8:
	case SDP_BOOL:
		src = (unsigned char *) &d->val.int8;
		data_size = sizeof(int8_t);
		break;
	case SDP_INT16:
		u16 = htons(d->val.int16);
		src = (unsigned char *) &u16;
		data_size = sizeof(int16_t);
		break;
	case SDP_INT32:
		u32 = htonl(d->val.int32);
		src = (unsigned char *) &u32;
		data_size = sizeof(int32_t);
		break;
	case SDP_INT64:
		u64 = hton64(d->val.int64);
		src = (unsigned char *) &u64;
		data_size = sizeof(int64_t);
		break;
	case SDP_INT128:
		hton128(&d->val.int128, &u128);
		src = (unsigned char *) &u128;
		data_size = sizeof(uint128_t);
		break;
	case SDP_TEXT_STR8:
	case SDP_TEXT_STR16:
	case SDP_TEXT_STR32:
	case SDP_URL_STR8:
	case SDP_URL_STR16:
	case SDP_URL_STR32:
		src = (unsigned char *) d->val.str;
		data_size = d->unitSize - sizeof(uint8_t);
		sdp_set_seq_len(seqp, data_size);
		break;
	case SDP_SEQ8:
	case SDP_SEQ16:
	case SDP_SEQ32:
		is_seq = 1;
		data_size = get_data_size(buf, d);
		sdp_set_seq_len(seqp, data_size);
		break;
	case SDP_ALT8:
	case SDP_ALT16:
	case SDP_ALT32:
		is_alt = 1;
		data_size = get_data_size(buf, d);
		sdp_set_seq_len(seqp, data_size);
		break;
	case SDP_UUID16:
		u16 = htons(d->val.uuid.value.uuid16);
		src = (unsigned char *) &u16;
		data_size = sizeof(uint16_t);
		break;
	case SDP_UUID32:
		u32 = htonl(d->val.uuid.value.uuid32);
		src = (unsigned char *) &u32;
		data_size = sizeof(uint32_t);
		break;
	case SDP_UUID128:
		src = (unsigned char *) &d->val.uuid.value.uuid128;
		data_size = sizeof(uint128_t);
		break;
	default:
		break;
	}

	if (!is_seq && !is_alt) {
		if (src && buf) {
			memcpy(buf->data + buf->data_size, src, data_size);
			buf->data_size += data_size;
		} else if (dtd != SDP_DATA_NIL)
			SDPDBG("Gen PDU : Cant copy from NULL source or dest\n");
	}

	pdu_size += data_size;

	return pdu_size;
}

static void sdp_attr_pdu(void *value, void *udata)
{
	sdp_append_to_pdu((sdp_buf_t *)udata, (sdp_data_t *)value);
}

int sdp_gen_record_pdu(const sdp_record_t *rec, sdp_buf_t *buf)
{
	buf->data = malloc(SDP_PDU_CHUNK_SIZE);
	if (!buf->data)
		return -ENOMEM;

	buf->buf_size = SDP_PDU_CHUNK_SIZE;
	buf->data_size = 0;
	memset(buf->data, 0, buf->buf_size);
	sdp_list_foreach(rec->attrlist, sdp_attr_pdu, buf);

	return 0;
}

void sdp_attr_replace(sdp_record_t *rec, uint16_t attr, sdp_data_t *d)
{
	sdp_data_t *p = sdp_data_get(rec, attr);

	if (p) {
		rec->attrlist = sdp_list_remove(rec->attrlist, p);
		sdp_data_free(p);
	}

	d->attrId = attr;
	rec->attrlist = sdp_list_insert_sorted(rec->attrlist, d, sdp_attrid_comp_func);

	if (attr == SDP_ATTR_SVCLASS_ID_LIST)
		extract_svclass_uuid(d, &rec->svclass);
}

int sdp_attrid_comp_func(const void *key1, const void *key2)
{
	const sdp_data_t *d1 = (const sdp_data_t *)key1;
	const sdp_data_t *d2 = (const sdp_data_t *)key2;

	if (d1 && d2)
		return d1->attrId - d2->attrId;
	return 0;
}

static void data_seq_free(sdp_data_t *seq)
{
	sdp_data_t *d = seq->val.dataseq;

	while (d) {
		sdp_data_t *next = d->next;
		sdp_data_free(d);
		d = next;
	}
}

void sdp_data_free(sdp_data_t *d)
{
	switch (d->dtd) {
	case SDP_SEQ8:
	case SDP_SEQ16:
	case SDP_SEQ32:
		data_seq_free(d);
		break;
	case SDP_URL_STR8:
	case SDP_URL_STR16:
	case SDP_URL_STR32:
	case SDP_TEXT_STR8:
	case SDP_TEXT_STR16:
	case SDP_TEXT_STR32:
		free(d->val.str);
		break;
	}
	free(d);
}

static sdp_data_t *extract_int(const void *p, int *len)
{
	sdp_data_t *d = malloc(sizeof(sdp_data_t));

	SDPDBG("Extracting integer\n");
	memset(d, 0, sizeof(sdp_data_t));
	d->dtd = *(uint8_t *) p;
	p += sizeof(uint8_t);
	*len += sizeof(uint8_t);

	switch (d->dtd) {
	case SDP_DATA_NIL:
		break;
	case SDP_BOOL:
	case SDP_INT8:
	case SDP_UINT8:
		*len += sizeof(uint8_t);
		d->val.uint8 = *(uint8_t *) p;
		break;
	case SDP_INT16:
	case SDP_UINT16:
		*len += sizeof(uint16_t);
		d->val.uint16 = ntohs(bt_get_unaligned((uint16_t *) p));
		break;
	case SDP_INT32:
	case SDP_UINT32:
		*len += sizeof(uint32_t);
		d->val.uint32 = ntohl(bt_get_unaligned((uint32_t *) p));
		break;
	case SDP_INT64:
	case SDP_UINT64:
		*len += sizeof(uint64_t);
		d->val.uint64 = ntoh64(bt_get_unaligned((uint64_t *) p));
		break;
	case SDP_INT128:
	case SDP_UINT128:
		*len += sizeof(uint128_t);
		ntoh128((uint128_t *) p, &d->val.uint128);
		break;
	default:
		free(d);
		d = NULL;
	}
	return d;
}

static sdp_data_t *extract_uuid(const uint8_t *p, int *len, sdp_record_t *rec)
{
	sdp_data_t *d = malloc(sizeof(sdp_data_t));

	SDPDBG("Extracting UUID");
	memset(d, 0, sizeof(sdp_data_t));
	if (sdp_uuid_extract(p, &d->val.uuid, len) < 0) {
		free(d);
		return NULL;
	}
	d->dtd = *(uint8_t *) p;
	if (rec)
		sdp_pattern_add_uuid(rec, &d->val.uuid);
	return d;
}

/*
 * Extract strings from the PDU (could be service description and similar info) 
 */
static sdp_data_t *extract_str(const void *p, int *len)
{
	char *s;
	int n;
	sdp_data_t *d = malloc(sizeof(sdp_data_t));

	memset(d, 0, sizeof(sdp_data_t));
	d->dtd = *(uint8_t *) p;
	p += sizeof(uint8_t);
	*len += sizeof(uint8_t);

	switch (d->dtd) {
	case SDP_TEXT_STR8:
	case SDP_URL_STR8:
		n = *(uint8_t *) p;
		p += sizeof(uint8_t);
		*len += sizeof(uint8_t) + n;
		break;
	case SDP_TEXT_STR16:
	case SDP_URL_STR16:
		n = ntohs(bt_get_unaligned((uint16_t *) p));
		p += sizeof(uint16_t);
		*len += sizeof(uint16_t) + n;
		break;
	default:
		SDPERR("Sizeof text string > UINT16_MAX\n");
		free(d);
		return 0;
	}

	s = malloc(n + 1);
	memset(s, 0, n + 1);
	memcpy(s, p, n);

	SDPDBG("Len : %d\n", n);
	SDPDBG("Str : %s\n", s);

	d->val.str = s;
	d->unitSize = n + sizeof(uint8_t);
	return d;
}

static sdp_data_t *extract_seq(const void *p, int *len, sdp_record_t *rec)
{
	int seqlen, n = 0;
	sdp_data_t *curr, *prev;
	sdp_data_t *d = malloc(sizeof(sdp_data_t));

	SDPDBG("Extracting SEQ");
	memset(d, 0, sizeof(sdp_data_t));
	*len = sdp_extract_seqtype(p, &d->dtd, &seqlen);
	SDPDBG("Sequence Type : 0x%x length : 0x%x\n", d->dtd, seqlen);

	if (*len == 0)
		return d;

	p += *len;
	curr = prev = NULL;
	while (n < seqlen) {
		int attrlen = 0;
		curr = sdp_extract_attr(p, &attrlen, rec);
		if (curr == NULL)
			break;

		if (prev)
			prev->next = curr;
		else
			d->val.dataseq = curr;
		prev = curr;
		p += attrlen;
		n += attrlen;

		SDPDBG("Extracted: %d SequenceLength: %d", n, seqlen);
	}

	*len += n;
	return d;
}

sdp_data_t *sdp_extract_attr(const uint8_t *p, int *size, sdp_record_t *rec)
{
	sdp_data_t *elem;
	int n = 0;
	uint8_t dtd = *(const uint8_t *)p;

	SDPDBG("extract_attr: dtd=0x%x", dtd);
	switch (dtd) {
	case SDP_DATA_NIL:
	case SDP_BOOL:
	case SDP_UINT8:
	case SDP_UINT16:
	case SDP_UINT32:
	case SDP_UINT64:
	case SDP_UINT128:
	case SDP_INT8:
	case SDP_INT16:
	case SDP_INT32:
	case SDP_INT64:
	case SDP_INT128:
		elem = extract_int(p, &n);
		break;
	case SDP_UUID16:
	case SDP_UUID32:
	case SDP_UUID128:
		elem = extract_uuid(p, &n, rec);
		break;
	case SDP_TEXT_STR8:
	case SDP_TEXT_STR16:
	case SDP_TEXT_STR32:
	case SDP_URL_STR8:
	case SDP_URL_STR16:
	case SDP_URL_STR32:
		elem = extract_str(p, &n);
		break;
	case SDP_SEQ8:
	case SDP_SEQ16:
	case SDP_SEQ32:
	case SDP_ALT8:
	case SDP_ALT16:
	case SDP_ALT32:
		elem = extract_seq(p, &n, rec);
		break;
	default:
		SDPERR("Unknown data descriptor : 0x%x terminating\n", dtd);
		return NULL;
	}
	*size += n;
	return elem;
}