sdp.c

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

		}
		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)
{
	uint128_t *base_uuid;
	char baseStr[128];
	int delim = '-';
	unsigned long dataLongValue;
	char *delimPtr;
	char *dataPtr;
	char temp[10];
	int toBeCopied;
	uint8_t *data;

	strcpy(baseStr, BASE_UUID);
	base_uuid = malloc(sizeof(uint128_t));
	if (!base_uuid)
		return NULL;

	data = 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);
		free(base_uuid);
		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);
		free(base_uuid);
		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);
		free(base_uuid);
		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);
		free(base_uuid);
		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 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
	uuid128->value.uuid128 = bluetooth_base_uuid;
	uuid128->type = SDP_UUID128;

	// extract bytes 2 and 3 of 128bit BT base UUID
	memcpy(&data1, &bluetooth_base_uuid.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
	uuid128->value.uuid128 = bluetooth_base_uuid;
	uuid128->type = SDP_UUID128;

	// extract first 4 bytes
	memcpy(&data0, &bluetooth_base_uuid.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 = bt_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)
{
	uint128_t *b = &bluetooth_base_uuid;
	uint128_t *u = &uuid->value.uuid128;
	uint32_t data;
	int i;

	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;
}

/*
 * This function appends data to the PDU buffer "dst" from source "src". 
 * The data length is also computed and set.
 * Should the PDU length exceed 2^8, then sequence type is
 * set accordingly and the data is memmove()'d.
 */
void sdp_append_to_buf(sdp_buf_t *dst, uint8_t *data, uint32_t len)
{
	uint8_t *p = dst->data;
	uint8_t dtd = *(uint8_t *) p;

	SDPDBG("Append src size: %d\n", len);
	SDPDBG("Append dst size: %d\n", dst->data_size);
	SDPDBG("Dst buffer size: %d\n", dst->buf_size);
	if (dst->data_size + len > dst->buf_size) {
		int need = SDP_PDU_CHUNK_SIZE * ((len / SDP_PDU_CHUNK_SIZE) + 1);
		dst->data = realloc(dst->data, dst->buf_size + need);

		SDPDBG("Realloc'ing : %d\n", need);

		if (dst->data == NULL) {
			SDPERR("Realloc fails \n");
		}
		dst->buf_size += need;
	}
	if (dst->data_size == 0 && dtd == 0) {
		// create initial sequence
		*(uint8_t *)p = SDP_SEQ8;
		p += sizeof(uint8_t);
		dst->data_size += sizeof(uint8_t);
		// reserve space for sequence size
		p += sizeof(uint8_t);
		dst->data_size += sizeof(uint8_t);
	}

	memcpy(dst->data + dst->data_size, data, len);
	dst->data_size += len;

	dtd = *(uint8_t *)dst->data;
	if (dst->data_size > UCHAR_MAX && dtd == SDP_SEQ8) {
		short offset = sizeof(uint8_t) + sizeof(uint8_t);
		memmove(dst->data + offset + 1, dst->data + offset, dst->data_size - offset);
		p = dst->data;
		*(uint8_t *) p = SDP_SEQ16;
		p += sizeof(uint8_t);
		dst->data_size += 1;
	}
	p = dst->data;
	dtd = *(uint8_t *) p;
	p += sizeof(uint8_t);
	switch (dtd) {
	case SDP_SEQ8:
		*(uint8_t *) p = dst->data_size - sizeof(uint8_t) - sizeof(uint8_t);
		break;
	case SDP_SEQ16:
		bt_put_unaligned(htons(dst->data_size - sizeof(uint8_t) - sizeof(uint16_t)), (uint16_t *) p);
		break;
	case SDP_SEQ32:
		bt_put_unaligned(htonl(dst->data_size - sizeof(uint8_t) - sizeof(uint32_t)), (uint32_t *) p);
		break;
	}
}

void sdp_append_to_pdu(sdp_buf_t *pdu, sdp_data_t *d)
{
	uint8_t buf[256];
	sdp_buf_t append;

	memset(&append, 0, sizeof(sdp_buf_t));
	append.data = buf;
	append.buf_size = sizeof(buf);
	append.data_size = 0;

	sdp_set_attrid(&append, d->attrId);
	sdp_gen_pdu(&append, d);
	sdp_append_to_buf(pdu, append.data, append.data_size);
}

/*
 * Registers an sdp record.
 *
 * It is incorrect to call this method on a record that
 * has been already registered with the server.
 *
 * Returns zero on success, otherwise -1 (and sets errno).
 */
int sdp_device_record_register_binary(sdp_session_t *session, bdaddr_t *device, uint8_t *data, uint32_t size, uint8_t flags, uint32_t *handle)
{
	uint8_t *req, *rsp, *p;
	uint32_t reqsize, rspsize;
	sdp_pdu_hdr_t *reqhdr, *rsphdr;
	int status;

	SDPDBG("");

	if (!session->local) {
		errno = EREMOTE;
		return -1;
	}
	req = malloc(SDP_REQ_BUFFER_SIZE);
	rsp = malloc(SDP_RSP_BUFFER_SIZE);
	if (req == NULL || rsp == NULL) {
		status = -1;
		errno = ENOMEM;
		goto end;
	}

	reqhdr = (sdp_pdu_hdr_t *)req;
	reqhdr->pdu_id = SDP_SVC_REGISTER_REQ;
	reqhdr->tid    = htons(sdp_gen_tid(session));
	reqsize = sizeof(sdp_pdu_hdr_t) + 1;
	p = req + sizeof(sdp_pdu_hdr_t);

	if (bacmp(device, BDADDR_ANY)) {
		*p++ = flags | SDP_DEVICE_RECORD;
		bacpy((bdaddr_t *) p, device);
		p += sizeof(bdaddr_t);
		reqsize += sizeof(bdaddr_t);
	} else
		*p++ = flags;

	memcpy(p, data, size);
	reqsize += size;
	reqhdr->plen = htons(reqsize - sizeof(sdp_pdu_hdr_t));

	status = sdp_send_req_w4_rsp(session, req, rsp, reqsize, &rspsize);
	if (status < 0)
		goto end;

	if (rspsize < sizeof(sdp_pdu_hdr_t)) {
		SDPERR("Unexpected end of packet");
		errno = EPROTO;
		status = -1;
		goto end;
	}

	rsphdr = (sdp_pdu_hdr_t *) rsp;
	p = rsp + sizeof(sdp_pdu_hdr_t);

	if (rsphdr->pdu_id == SDP_ERROR_RSP) {
		/* Invalid service record */
		errno = EINVAL;
		status = -1;
	} else if (rsphdr->pdu_id != SDP_SVC_REGISTER_RSP) {
		errno = EPROTO;
		status = -1;
	} else {
		if (rspsize < sizeof(sdp_pdu_hdr_t) + sizeof(uint32_t)) {
			SDPERR("Unexpected end of packet");
			errno = EPROTO;
			status = -1;
			goto end;
		}
		if (handle)
			*handle  = ntohl(bt_get_unaligned((uint32_t *) p));
	}

end:
	if (req)
		free(req);

	if (rsp)
		free(rsp);

	return status;
}

int sdp_device_record_register(sdp_session_t *session, bdaddr_t *device, sdp_record_t *rec, uint8_t flags)
{
	sdp_buf_t pdu;
	uint32_t handle;
	int err;

	SDPDBG("");

	if (rec->handle && rec->handle != 0xffffffff) {
		uint32_t handle = rec->handle;
		sdp_data_t *data = sdp_data_alloc(SDP_UINT32, &handle);
		sdp_attr_replace(rec, SDP_ATTR_RECORD_HANDLE, data);
	}

	if (sdp_gen_record_pdu(rec, &pdu) < 0) {
		errno = ENOMEM;
		return -1;
	}

	err = sdp_device_record_register_binary(session, device,
				pdu.data, pdu.data_size, flags, &handle);

	free(pdu.data);

	if (err == 0) {
		sdp_data_t *data = sdp_data_alloc(SDP_UINT32, &handle);
		rec->handle = handle;
		sdp_attr_replace(rec, SDP_ATTR_RECORD_HANDLE, data);
	}

	return err;
}

int sdp_record_register(sdp_session_t *session, sdp_record_t *rec, uint8_t flags)
{
	return sdp_device_record_register(session, BDADDR_ANY, rec, flags);
}

/*
 * unregister a service record
 */
int sdp_device_record_unregister_binary(sdp_session_t *session, bdaddr_t *device, uint32_t handle)
{
	uint8_t *reqbuf, *rspbuf, *p;
	uint32_t reqsize = 0, rspsize = 0;
	sdp_pdu_hdr_t *reqhdr, *rsphdr;
	int status;

	SDPDBG("");

	if (handle == SDP_SERVER_RECORD_HANDLE) {
		errno = EINVAL;
		return -1;
	}

	if (!session->local) {
		errno = EREMOTE;
		return -1;
	}

	reqbuf = malloc(SDP_REQ_BUFFER_SIZE);
	rspbuf = malloc(SDP_RSP_BUFFER_SIZE);
	if (!reqbuf || !rspbuf) {
		errno = ENOMEM;
		status = -1;
		goto end;
	}
	reqhdr = (sdp_pdu_hdr_t *) reqbuf;
	reqhdr->pdu_id = SDP_SVC_REMOVE_REQ;
	reqhdr->tid    = htons(sdp_gen_tid(session));

	p = reqbuf + sizeof(sdp_pdu_hdr_t);
	reqsize = sizeof(sdp_pdu_hdr_t);
	bt_put_unaligned(htonl(handle), (uint32_t *) p);
	reqsize += sizeof(uint32_t);

	reqhdr->plen = htons(reqsize - sizeof(sdp_pdu_hdr_t));
	status = sdp_send_req_w4_rsp(session, reqbuf, rspbuf, reqsize, &rspsize);
	if (status < 0)
		goto end;

	if (rspsize < sizeof(sdp_pdu_hdr_t) + sizeof(uint16_t)) {
		SDPERR("Unexpected end of packet");
		errno = EPROTO;
		status = -1;
		goto end;
	}

	rsphdr = (sdp_pdu_hdr_t *) rspbuf;
	p = rspbuf + sizeof(sdp_pdu_hdr_t);
	status = bt_get_unaligned((u