sdp.c

蓝牙blue-libs-2.25.tar.gz网上很少找到的 了

 * 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 uint8_t *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(bt_get_unaligned((uint16_t *) p)));
		*scanned += sizeof(uint16_t);
		p += sizeof(uint16_t);
	} else if (type == SDP_UUID32) {
		sdp_uuid32_create(uuid, ntohl(bt_get_unaligned((uint32_t *) p)));
		*scanned += sizeof(uint32_t);
		p += sizeof(uint32_t);
	} else {
		sdp_uuid128_create(uuid, p);
		*scanned += sizeof(uint128_t);
		p += sizeof(uint128_t);
	}
	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[SDP_SEQ_PDUFORM_SIZE];
	sdp_buf_t append;

	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(sdp_session_t *session, bdaddr_t *device, sdp_record_t *rec, uint8_t flags)
{
	int status = 0;
	uint8_t *req, *rsp, *p;
	uint32_t reqsize, rspsize;
	sdp_pdu_hdr_t *reqhdr, *rsphdr;
	sdp_buf_t pdu;

	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;
	}
	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);
	}
	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;
	if (sdp_gen_record_pdu(rec, &pdu) < 0) {
		status = -1;
		errno = ENOMEM;
		goto end;
	}
	memcpy(p, pdu.data, pdu.data_size);
	free(pdu.data);
	reqsize += pdu.data_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;
	rsphdr = (sdp_pdu_hdr_t *) rsp;
	p = rsp + sizeof(sdp_pdu_hdr_t);
	if (rsphdr->pdu_id == SDP_SVC_REGISTER_RSP) {
		uint32_t handle  = ntohl(bt_get_unaligned((uint32_t *) p));
		sdp_data_t *data = sdp_data_alloc(SDP_UINT32, &handle);
		rec->handle = handle;
		sdp_attr_replace(rec, SDP_ATTR_RECORD_HANDLE, data);
	}
end:
	if (req)
		free(req);
	if (rsp)
		free(rsp);
	return status;
}

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(sdp_session_t *session, bdaddr_t *device, sdp_record_t *rec)
{
	int status = 0;
	uint8_t *reqbuf, *rspbuf, *p;
	uint32_t reqsize = 0, rspsize = 0;
	sdp_pdu_hdr_t *reqhdr, *rsphdr;
	uint32_t handle = 0;

	SDPDBG("");

	handle = rec->handle;
	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) {
		rsphdr = (sdp_pdu_hdr_t *) rspbuf;
		p = rspbuf + sizeof(sdp_pdu_hdr_t);
		status = bt_get_unaligned((uint16_t *) p);
		if (status == 0 && rsphdr->pdu_id == SDP_SVC_REMOVE_RSP) {
			SDPDBG("Removing local copy\n");
			sdp_record_free(rec);
		}
	}
end:
	if (reqbuf)
		free(reqbuf);
	if (rspbuf)
		free(rspbuf);
	return status;
}

int sdp_record_unregister(sdp_session_t *session, sdp_record_t *rec)
{
	return sdp_device_record_unregister(session, BDADDR_ANY, rec);
}

/*
 * modify an existing service record
 */
int sdp_device_record_update(sdp_session_t *session, bdaddr_t *device, const sdp_record_t *rec)
{
	int status = 0;
	uint8_t *reqbuf, *rspbuf, *p;
	uint32_t reqsize, rspsize;
	sdp_pdu_hdr_t *reqhdr, *rsphdr;
	uint32_t handle;
	sdp_buf_t pdu;

	SDPDBG("");
	handle = rec->handle;

	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_UPDATE_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);
	p += sizeof(uint32_t);

	if (0 > sdp_gen_record_pdu(rec, &pdu)) {
		errno = ENOMEM;
		status = -1;
		goto end;
	}
	memcpy(p, pdu.data, pdu.data_size);
	reqsize += pdu.data_size;

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

	SDPDBG("Send req status : %d\n", status);

	if (status == 0) {
		rsphdr = (sdp_pdu_hdr_t *) rspbuf;
		p = rspbuf + sizeof(sdp_pdu_hdr_t);
		status = bt_get_unaligned((uint16_t *) p);
	}
end:
	if (reqbuf)
		free(reqbuf);
	if (rspbuf)
		free(rspbuf);
	return status;
}

int sdp_record_update(sdp_session_t *session, const sdp_record_t *rec)
{
	return sdp_device_record_update(session, BDADDR_ANY, rec);
}

sdp_record_t *sdp_record_alloc()
{
	sdp_record_t *rec = (sdp_record_t *)malloc(sizeof(sdp_record_t));
	memset((void *)rec, 0, sizeof(sdp_record_t));
	rec->handle = 0xffffffff;
	return rec;
}

/*
 * Free the contents of a service record
 */
void sdp_record_free(sdp_record_t *rec)
{
	sdp_list_free(rec->attrlist, (sdp_free_func_t)sdp_data_free);
	sdp_list_free(rec->pattern, free);
	free(rec);
}

void sdp_pattern_add_uuid(sdp_record_t *rec, uuid_t *uuid)
{
	uuid_t *uuid128 = sdp_uuid_to_uuid128(uuid);

	SDPDBG("SvcRec : 0x%lx\n", (unsigned long)rec);
	SDPDBG("Elements in target pattern : %d\n", sdp_list_len(rec->pattern));
	SDPDBG("Trying to add : 0x%lx\n", (unsigned long)uuid128);

	if (sdp_list_find(rec->pattern, uuid128, sdp_uuid128_cmp) == NULL)
		rec->pattern = sdp_list_insert_sorted(rec->pattern, uuid128, sdp_uuid128_cmp);
	else
		free(uuid128);

	SDPDBG("Elements in target pattern : %d\n", sdp_list_len(rec->pattern));
}

void sdp_pattern_add_uuidseq(sdp_record_t *rec, sdp_list_t *seq)
{
	for (; seq; seq = seq->next) {
		uuid_t *uuid = (uuid_t *)seq->data;
		sdp_pattern_add_uuid(rec, uuid);
	}
}

/*
 * Extract a sequence of service record handles from a PDU buffer
 * and add the entries to a sdp_list_t. Note that the service record
 * handles are not in "data element sequence" form, but just like
 * an array of service handles
 */
static void extract_record_handle_seq(uint8_t *pdu, sdp_list_t **seq, int count, int *scanned)
{
	sdp_list_t *pSeq = *seq;
	uint8_t *pdata = pdu;
	int n;

	for (n = 0; n < count; n++) {
		uint32_t *pSvcRec = (uint32_t *) malloc(sizeof(uint32_t));
		*pSvcRec = ntohl(bt_get_unaligned((uint32_t *) pdata));
		pSeq = sdp_list_append(pSeq, pSvcRec);
		pdata += sizeof(uint32_t);
		*scanned += sizeof(uint32_t);
	}
	*seq = pSeq;
}
/*
 * Generate the attribute sequence pdu form
 * from sdp_list_t elements. Return length of attr seq
 */
static int gen_dataseq_pdu(uint8_t *dst, const sdp_list_t *seq, uint8_t dtd)
{
	sdp_data_t *dataseq;
	void **types, **values;
	sdp_buf_t buf;
	int i, seqlen = sdp_list_len(seq);

	// Fill up the value and the dtd arrays
	SDPDBG("");
	
	memset(&buf, 0, sizeof(sdp_buf_t));
	buf.