sdp.c

这是Linux环境下的蓝牙源代码

	if (bufsize < n) {
		SDPERR("String too long to fit in packet");
		free(d);
		return NULL;
	}

	s = malloc(n + 1);
	if (!s) {
		SDPERR("Not enough memory for incoming string");
		free(d);
		return NULL;
	}
	memset(s, 0, n + 1);
	memcpy(s, p, n);

	*len += n;

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

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

/*
 * Extract the sequence type and its length, and return offset into buf
 * or 0 on failure.
 */
int sdp_extract_seqtype(const uint8_t *buf, int bufsize, uint8_t *dtdp, int *size)
{
	uint8_t dtd;
	int scanned = sizeof(uint8_t);

	if (bufsize < sizeof(uint8_t)) {
		SDPERR("Unexpected end of packet");
		return 0;
	}

	dtd = *(uint8_t *) buf;
	buf += sizeof(uint8_t);
	bufsize -= sizeof(uint8_t);
	*dtdp = dtd;
	switch (dtd) {
	case SDP_SEQ8:
	case SDP_ALT8:
		if (bufsize < sizeof(uint8_t)) {
			SDPERR("Unexpected end of packet");
			return 0;
		}
		*size = *(uint8_t *) buf;
		scanned += sizeof(uint8_t);
		break;
	case SDP_SEQ16:
	case SDP_ALT16:
		if (bufsize < sizeof(uint16_t)) {
			SDPERR("Unexpected end of packet");
			return 0;
		}
		*size = ntohs(bt_get_unaligned((uint16_t *) buf));
		scanned += sizeof(uint16_t);
		break;
	case SDP_SEQ32:
	case SDP_ALT32:
		if (bufsize < sizeof(uint32_t)) {
			SDPERR("Unexpected end of packet");
			return 0;
		}
		*size = ntohl(bt_get_unaligned((uint32_t *) buf));
		scanned += sizeof(uint32_t);
		break;
	default:
		SDPERR("Unknown sequence type, aborting\n");
		return 0;
	}
	return scanned;
}

static sdp_data_t *extract_seq(const void *p, int bufsize, 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, bufsize, &d->dtd, &seqlen);
	SDPDBG("Sequence Type : 0x%x length : 0x%x\n", d->dtd, seqlen);

	if (*len == 0)
		return d;

	if (*len > bufsize) {
		SDPERR("Packet not big enough to hold sequence.");
		free(d);
		return NULL;
	}

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

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

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

	*len += n;
	return d;
}

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

	if (bufsize < sizeof(uint8_t)) {
		SDPERR("Unexpected end of packet");
		return NULL;
	}

	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, bufsize, &n);
		break;
	case SDP_UUID16:
	case SDP_UUID32:
	case SDP_UUID128:
		elem = extract_uuid(p, bufsize, &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, bufsize, &n);
		break;
	case SDP_SEQ8:
	case SDP_SEQ16:
	case SDP_SEQ32:
	case SDP_ALT8:
	case SDP_ALT16:
	case SDP_ALT32:
		elem = extract_seq(p, bufsize, &n, rec);
		break;
	default:
		SDPERR("Unknown data descriptor : 0x%x terminating\n", dtd);
		return NULL;
	}
	*size += n;
	return elem;
}

#ifdef SDP_DEBUG
static void attr_print_func(void *value, void *userData)
{
	sdp_data_t *d = (sdp_data_t *)value;

	SDPDBG("=====================================\n");
	SDPDBG("ATTRIBUTE IDENTIFIER : 0x%x\n",  d->attrId);
	SDPDBG("ATTRIBUTE VALUE PTR : 0x%x\n", (uint32_t)value);
	if (d)
		sdp_data_print(d);
	else
		SDPDBG("NULL value\n");
	SDPDBG("=====================================\n");
}

void sdp_print_service_attr(sdp_list_t *svcAttrList)
{
	SDPDBG("Printing service attr list %p\n", svcAttrList);
	sdp_list_foreach(svcAttrList, attr_print_func, NULL);
	SDPDBG("Printed service attr list %p\n", svcAttrList);
}
#endif

sdp_record_t *sdp_extract_pdu(const uint8_t *buf, int bufsize, int *scanned)
{
	int extracted = 0, seqlen = 0;
	uint8_t dtd;
	uint16_t attr;
	sdp_record_t *rec = sdp_record_alloc();
	const uint8_t *p = buf;

	*scanned = sdp_extract_seqtype(buf, bufsize, &dtd, &seqlen);
	p += *scanned;
	bufsize -= *scanned;
	rec->attrlist = NULL;

	while (extracted < seqlen && bufsize > 0) {
		int n = sizeof(uint8_t), attrlen = 0;
		sdp_data_t *data = NULL;

		SDPDBG("Extract PDU, sequenceLength: %d localExtractedLength: %d",
							seqlen, extracted);

		if (bufsize < n + sizeof(uint16_t)) {
			SDPERR("Unexpected end of packet");
			break;
		}

		dtd = *(uint8_t *) p;
		attr = ntohs(bt_get_unaligned((uint16_t *) (p + n)));
		n += sizeof(uint16_t);

		SDPDBG("DTD of attrId : %d Attr id : 0x%x \n", dtd, attr);

		data = sdp_extract_attr(p + n, bufsize - n, &attrlen, rec);

		SDPDBG("Attr id : 0x%x attrValueLength : %d\n", attr, attrlen);

		n += attrlen;
		if (data == NULL) {
			SDPDBG("Terminating extraction of attributes");
			break;
		}

		if (attr == SDP_ATTR_RECORD_HANDLE)
			rec->handle = data->val.uint32;

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

		extracted += n;
		p += n;
		bufsize -= n;
		sdp_attr_replace(rec, attr, data);

		SDPDBG("Extract PDU, seqLength: %d localExtractedLength: %d",
							seqlen, extracted);
	}
#ifdef SDP_DEBUG
	SDPDBG("Successful extracting of Svc Rec attributes\n");
	sdp_print_service_attr(rec->attrlist);
#endif
	*scanned += seqlen;
	return rec;
}

#ifdef SDP_DEBUG
static void print_dataseq(sdp_data_t *p)
{
	sdp_data_t *d;

	for (d = p; d; d = d->next)
		sdp_data_print(d);
}
#endif

void sdp_record_print(const sdp_record_t *rec)
{
	sdp_data_t *d = sdp_data_get(rec, SDP_ATTR_SVCNAME_PRIMARY);
	if (d)
		printf("Service Name: %.*s\n", d->unitSize, d->val.str);
	d = sdp_data_get(rec, SDP_ATTR_SVCDESC_PRIMARY);
	if (d)
		printf("Service Description: %.*s\n", d->unitSize, d->val.str);
	d = sdp_data_get(rec, SDP_ATTR_PROVNAME_PRIMARY);
	if (d)
		printf("Service Provider: %.*s\n", d->unitSize, d->val.str);
}

#ifdef SDP_DEBUG
void sdp_data_print(sdp_data_t *d)
{
	switch (d->dtd) {
	case SDP_DATA_NIL:
		SDPDBG("NIL\n");
		break;
	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:
		SDPDBG("Integer : 0x%x\n", d->val.uint32);
		break;
	case SDP_UUID16:
	case SDP_UUID32:
	case SDP_UUID128:
		SDPDBG("UUID\n");
		sdp_uuid_print(&d->val.uuid);
		break;
	case SDP_TEXT_STR8:
	case SDP_TEXT_STR16:
	case SDP_TEXT_STR32:
		SDPDBG("Text : %s\n", d->val.str);
		break;
	case SDP_URL_STR8:
	case SDP_URL_STR16:
	case SDP_URL_STR32:
		SDPDBG("URL : %s\n", d->val.str);
		break;
	case SDP_SEQ8:
	case SDP_SEQ16:
	case SDP_SEQ32:
		print_dataseq(d->val.dataseq);
		break;
	case SDP_ALT8:
	case SDP_ALT16:
	case SDP_ALT32:
		SDPDBG("Data Sequence Alternates\n");
		print_dataseq(d->val.dataseq);
		break;
	}
}
#endif

sdp_data_t *sdp_data_get(const sdp_record_t *rec, uint16_t attrId)
{
	if (rec->attrlist) {
		sdp_data_t sdpTemplate;
		sdp_list_t *p;

		sdpTemplate.attrId = attrId;
		p = sdp_list_find(rec->attrlist, &sdpTemplate, sdp_attrid_comp_func);
		if (p)
			return (sdp_data_t *)p->data;
	}
	return NULL;
}

int sdp_send_req(sdp_session_t *session, uint8_t *buf, uint32_t size)
{
	uint32_t sent = 0;

	while (sent < size) {
		int n = send(session->sock, buf + sent, size - sent, 0);
		if (n < 0)
			return -1;
		sent += n;
	}
	return 0;
}

int sdp_read_rsp(sdp_session_t *session, uint8_t *buf, uint32_t size)
{
	fd_set readFds;
	struct timeval timeout = { SDP_RESPONSE_TIMEOUT, 0 };

	FD_ZERO(&readFds);
	FD_SET(session->sock, &readFds);
	SDPDBG("Waiting for response\n");
	if (select(session->sock + 1, &readFds, NULL, NULL, &timeout) == 0) {
		SDPERR("Client timed out\n");
		errno = ETIMEDOUT;
		return -1;
	}
	return recv(session->sock, buf, size, 0);
}

/*
 * generic send request, wait for response method.
 */
int sdp_send_req_w4_rsp(sdp_session_t *session, uint8_t *reqbuf, uint8_t *rspbuf, uint32_t reqsize, uint32_t *rspsize)
{
	int n;
	sdp_pdu_hdr_t *reqhdr = (sdp_pdu_hdr_t *)reqbuf;
	sdp_pdu_hdr_t *rsphdr = (sdp_pdu_hdr_t *)rspbuf;

	SDPDBG("");
	if (0 > sdp_send_req(session, reqbuf, reqsize)) {
		SDPERR("Error sending data:%s", strerror(errno));
		return -1;
	}
	n = sdp_read_rsp(session, rspbuf, SDP_RSP_BUFFER_SIZE);
	if (0 > n)
		return -1;
	SDPDBG("Read : %d\n", n);
	if (n == 0 || reqhdr->tid != rsphdr->tid) {
		errno = EPROTO;
		return -1;
	}
	*rspsize = n;
	return 0;
}

/*
 * singly-linked lists (after openobex implementation)
 */
sdp_list_t *sdp_list_append(sdp_list_t *p, void *d)
{
	sdp_list_t *q, *n = malloc(sizeof(sdp_list_t));

	if (!n)
		return 0;

	n->data = d;
	n->next = 0;

	if (!p)
		return n;

	for (q = p; q->next; q = q->next);
	q->next = n;

	return p;
}

sdp_list_t *sdp_list_remove(sdp_list_t *list, void *d)
{
	sdp_list_t *p, *q;

	for (q = 0, p = list; p; q = p, p = p->next)
		if (p->data == d) {
			if (q)
				q->next = p->next;
			else
				list = p->next;
			free(p);
			break;
		}

	return list;
}

sdp_list_t *sdp_list_insert_sorted(sdp_list_t *list, void *d, sdp_comp_func_t f)
{
	sdp_list_t *q, *p, *n;

	n = malloc(sizeof(sdp_list_t));
	if (!n)
		return 0;
	n->data = d;
	for (q = 0, p = list; p; q = p, p = p->next)
		if (f(p->data, d) >= 0)
			break; 
	// insert between q and p; if !q insert at head
	if (q)
		q->next = n;
	else
		list = n;
	n->next = p;
	return list;
}

/*
 * Every element of the list points to things which need 
 * to be free()'d. This method frees the list's contents
 */
void sdp_list_free(sdp_list_t *list, sdp_free_func_t f)
{
	sdp_list_t *next;
	while (list) {
		next = list->next;
		if (f)
			f(list->data);
		free(list);
		list = next;
	}
}

static inline int __find_port(sdp_data_t *seq, int proto)
{
	if (!seq || !seq->next)
		return 0;

	if (SDP_IS_UUID(seq->dtd) && sdp_uuid_to_proto(&seq->val.uuid) == proto) {
		seq = seq->next;
		switch (seq->dtd) {
		case SDP_UINT8:
			return seq->val.uint8;
		case SDP_UINT16:
			return seq->val.uint16;
		}
	}
	return 0;
}

int sdp_get_proto_port(const sdp_list_t *list, int proto)
{
	if (proto != L2CAP_UUID && proto != RFCOMM_UUID) {
		errno = EINVAL;
		return -1;
	}

	for (; list; list = list->next) {
		sdp_list_t *p;
		for (p = list->data; p; p = p->next) {
			sdp_data_t *seq = (sdp_data_t *) p->data;
			int port = __find_port(seq, proto);
			if (port)
				return port;
		}
	}
	return 0;
}

sdp_data_t *sdp_get_proto_desc(sdp_list_t *list, int proto)
{
	for (; list; list = list->next) {
		sdp_list_t *p;
		for (p = list->data; p; p = p->next) {
			sdp_data_t *seq = (sdp_data_t *) p->data;
			if (SDP_IS_UUID(seq->dtd) && 
					sdp_uuid_to_proto(&seq->val.uuid) == proto)
				return seq->next;
		}
	}
	return NULL;
}

int sdp_get_access_protos(const sdp_record_t *rec, sdp_list_t **pap)
{
	sdp_data_t *pdlist, *curr;
	sdp_list_t *ap = 0;

	pdlist = sdp_data_get(rec, SDP_ATTR_PROTO_DESC_LIST);
	if (pdlist == NULL) {
		errno = ENODATA;
		return -1;
	}
	SDPDBG("AP type : 0%x\n", pdlist->dtd);

	for (; pdlist; pdlist = pdlist->next) {
		sdp_list_t *pds = 0;
		for (curr = pdlist->val.dataseq; curr; curr = curr->next)