sdp.c

linux下的蓝牙协议栈

		*scanned += sizeof(uint16_t);
		p += sizeof(uint16_t);
	} else if (type == SDP_UUID32) {
		sdp_uuid32_create(uuid, ntohl(sdp_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, char *data, int len)
{
	char *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 = (char *)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:
		sdp_put_unaligned(htons(dst->data_size - sizeof(uint8_t) - sizeof(uint16_t)), (uint16_t *)p);
		break;
	case SDP_SEQ32:
		sdp_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)
{
	char 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 a non-null value (a pointer) to a service
 * record if successful, else -1 setting errno
 */
int sdp_record_register(sdp_session_t *session, sdp_record_t *rec, uint8_t flags)
{
	char *p;
	int status = 0;
	char *req, *rsp;
	int reqsize, rspsize;
	sdp_pdu_hdr_t *reqhdr, *rsphdr;
	sdp_buf_t pdu;

	SDPDBG("");

	if (!session->local) {
		errno = EREMOTE;
		return -1;
	}
	req = (char *)malloc(SDP_REQ_BUFFER_SIZE);
	rsp = (char *)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);
	*p++ = flags;
	if (0 > sdp_gen_record_pdu(rec, &pdu)) {
		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(sdp_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;
}

/*
 * unregister a service record
 */
int sdp_record_unregister(sdp_session_t *session, sdp_record_t *rec)
{
	char *p;
	int status = 0;
	char *reqbuf, *rspbuf;
	int 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 = (char *)malloc(SDP_REQ_BUFFER_SIZE);
	rspbuf = (char *)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);
	sdp_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 = sdp_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;
}

/*
 * modify an existing service record
 */
int sdp_record_update(sdp_session_t *session, const sdp_record_t *rec)
{
	char *p;
	int status = 0;
	char *reqbuf, *rspbuf;
	int 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 = (char *)malloc(SDP_REQ_BUFFER_SIZE);
	rspbuf = (char *)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 = (char *)(reqbuf + sizeof(sdp_pdu_hdr_t));
	reqsize = sizeof(sdp_pdu_hdr_t);

	sdp_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 = sdp_get_unaligned((uint16_t *)p);
	}
end:
	if (reqbuf)
		free(reqbuf);
	if (rspbuf)
		free(rspbuf);
	return status;
}

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(char *pdu, sdp_list_t **seq, int count, int *scanned)
{
	sdp_list_t *pSeq = *seq;
	char *pdata = pdu;
	int n;

	for (n = 0; n < count; n++) {
		uint32_t *pSvcRec = (uint32_t *) malloc(sizeof(uint32_t));
		*pSvcRec = ntohl(sdp_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(char *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.data = (char *)malloc(SDP_UUID_SEQ_SIZE);
	buf.buf_size = SDP_UUID_SEQ_SIZE;

	SDPDBG("Seq length : %d\n", seqlen);

	types = malloc(seqlen * sizeof(void *));
	values = malloc(seqlen * sizeof(void *));
	for (i = 0; i < seqlen; i++) {
		void *data = seq->data;
		types[i] = &dtd;
		if (SDP_IS_UUID(dtd))
			data = &((uuid_t *)data)->value;
		values[i] = data;
		seq = seq->next;
	}

	dataseq = sdp_seq_alloc(types, values, seqlen);
	SDPDBG("Data Seq : 0x%p\n", seq);
	seqlen = sdp_gen_pdu(&buf, dataseq);
	SDPDBG("Copying : %d\n", buf.data_size);
	memcpy(dst, buf.data, buf.data_size);

	sdp_data_free(dataseq);

	free(types);
	free(values);
	free(buf.data);
	return seqlen;
}

static int gen_searchseq_pdu(char *dst, const sdp_list_t *seq)
{
	uuid_t *uuid = (uuid_t *)seq->data;
	return gen_dataseq_pdu(dst, seq, uuid->type);
}

static int gen_attridseq_pdu(char *dst, const sdp_list_t *seq, uint8_t dataType)
{
	return gen_dataseq_pdu(dst, seq, dataType);
}

static int copy_cstate(char *pdata, const sdp_cstate_t *cstate)
{
	if (cstate) {
		*pdata++ = cstate->length;
		memcpy(pdata, cstate->data, cstate->length);
		return cstate->length + 1;
	}
	*pdata = 0;
	return 1;
}

/*
 * This is a service search request. 
 *
 * INPUT :
 *
 *   sdp_list_t *search_list
 *     Singly linked list containing elements of the search
 *     pattern. Each entry in the list is a UUID (DataTypeSDP_UUID16)
 *     of the service to be searched
 *
 *   uint16_t max_rec_num
 *      A 16 bit integer which tells the service, the maximum
 *      entries that the client can handle in the response. The
 *      server is obliged not to return > max_rec_num entries
 *
 * OUTPUT :
 *
 *   int return value
 *     0:
 *       The request completed successfully. This does not
 *       mean the requested services were found
 *     -1:
 *       On any failure and sets errno
 *
 *   sdp_list_t **rsp_list
 *     This variable is set on a successful return if there are
 *     non-zero service handles. It is a singly linked list of
 *     service records (sdp_record_t *)
 *
 *   uint16_t *handleCount
 *     This is a pointer to a 16 bit integer, which is set to 
 *     indicate the number of service record handles present in
 *     rec_list
 */
int sdp_service_search_req(sdp_session_t *session, const sdp_list_t *search,
			uint16_t max_rec_num, sdp_list_t **rsp)
{
	int status = 0;
	int reqsize = 0, _reqsize;
	int rspsize = 0, rsplen;
	int seqlen = 0;
	int scanned, total_rec_count, rec_count;
	char *pdata, *_pdata;
	char *reqbuf, *rspbuf;
	sdp_pdu_hdr_t *reqhdr, *rsphdr;
	sdp_cstate_t *cstate = NULL;

	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_SEARCH_REQ;
	pdata = reqbuf + sizeof(sdp_pdu_hdr_t);
	reqsize = sizeof(sdp_pdu_hdr_t);

	// add service class IDs for search
	seqlen = gen_searchseq_pdu(pdata, search);

	SDPDBG("Data seq added : %d\n", seqlen);

	// set the length and increment the pointer
	reqsize += seqlen;
	pdata += seqlen;

	// specify the maximum svc rec count that client expects
	sdp_put_unaligned(htons(max_rec_num), (uint16_t *)pdata);
	reqsize += sizeof(uint16_t);
	pdata += sizeof(uint16_t);

	_reqsize = reqsize;
	_pdata   = pdata;
	*rsp = NULL;

	do {
		// Add continuation state or NULL (first time)
		reqsize = _reqsize + copy_cstate(_pdata, cstate);

		// Set the request header's param length
		reqhdr->plen = htons(reqsize - sizeof(sdp_pdu_hdr_t));

		reqhdr->tid  = htons(sdp_gen_tid(session));
		/*
		 * Send the request, wait for response and if
		 * no error, set the appropriate values and return
		 */
		status = sdp_send_req_w4_rsp(session, reqbuf, rspbuf, reqsize, &rspsize);
		if (status < 0)
			goto end;

		rsplen = 0;
		rsphdr = (sdp_pdu_hdr_t *)rspbuf;
		rsplen = ntohs(rsphdr->plen);

		if (rsphdr->pdu_id == SDP_ERROR_RSP) {
			SDPDBG("Status : 0x%x\n", rsphdr->pdu_id);
			status = -1;
			goto end;
		}
		scanned = 0;
		pdata = rspbuf + sizeof(sdp_pdu_hdr_t);

		// net service record match count
		total_rec_count = ntohs(sdp_get_unaligned((uint16_t *)pdata));
		pdata += sizeof(uint16_t);
		scanned += sizeof(uint16_t);
		rec_count = ntohs(sdp_get_unaligned((uint16_t *)pdata));
		pdata += sizeof(uint16_t);
		scanned += sizeof(uint16_t);

		SDPDBG("Total svc count: %d\n", total_rec_count);
		SDPDBG("Current sv